Repository navigation
fix(mpc_lateral_controller): keep the steering state within the steering limit - #3584
Draft
danielsanchezaran wants to merge 3 commits into
Draft
danielsanchezaran wants to merge 3 commits into
danielsanchezaran wants to merge 3 commits into
Conversation
…ing limit
While Autoware is not in control, the controller starts from the measured steering. When the
driver steers past the steering limit, the low-confidence slew limit publishes a command beyond the
limit, and syncMpcSteerStateToCommand writes it into the MPC state (m_raw_steer_cmd_prev). The
first steering-rate constraint of the next QP then excludes every angle within the limit, so the QP
is infeasible ("qp solver error"). The failure path publishes the last command and syncs it again,
so the MPC stays failed with the same out-of-range command until the vehicle stops. When control is
enabled meanwhile, the command gate steers toward that stale command.
- resetSteeringCmdFilter clamps to the steering limit, as resetPrevResult already does.
- getInitialControlCommand clamps the measured steering to the limit.
- When the QP fails while Autoware is not in control, follow the measured steering instead of
holding the last command.
- Test: a solve after syncing a command beyond the limit succeeds (primal infeasible before).
Signed-off-by: Daniel <daniel.sanchez@tier4.jp>
…hout the offset (review) - On a QP failure while Autoware is not in control, the measured-steering command is offset-compensated like the MPC state; subtract the steering offset before publishing, as for every other command (with enable_auto_steering_offset_removal it was off by the offset). - Log which fallback is used. - Controller test: the driver steers beyond the limit in manual with a low-confidence reference, straightens the wheel and engages; the command must follow the wheel. Without the fix it stays at 0.767 rad. Signed-off-by: Daniel <daniel.sanchez@tier4.jp>
…e latch test Signed-off-by: Daniel <daniel.sanchez@tier4.jp>
|
|
These three PRs are one vehicle-test set (MPC latch + ideal planner + its launch config), but the bases are not on one line:
Please retarget the whole set onto one line before the next test image is cut:
Same note on the other two PRs. |
This branch has not been deployed
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Description
Problem. If the driver steers beyond the MPC steering limit (36.67 deg = 0.640 rad) while in manual, the MPC locks onto a command beyond the limit and its QP fails every cycle until the vehicle stops. When Autoware control is engaged during that time, the wheel is steered toward the stale command, even if the driver had already straightened the wheel.
Seen on the vehicle (J6 gen2, 2026-10-06 14:24:27-14:25:56 JST):
optimization failure (qp solver error)for 90 s.Root cause.
m_ctrl_cmd_prev= measured steering (40.12 deg).applyConfidenceSteerSlewLimitmoves one step from it and publishes 39.58 deg. Nothing clamps it to the limit.syncMpcSteerStateToCommandwrites 39.58 deg intom_raw_steer_cmd_prev. The next QP's first-step rate constraint (prev ± rate * ctrl_period) cannot be met by any angle within±m_steer_lim→ primal infeasible.getStopControlCommand()(= the last command, 39.58 deg) and syncs it again → every following QP is infeasible too.Fix.
MPC::resetSteeringCmdFilter: clamp to±m_steer_lim(asresetPrevResultalready does). This breaks the latch.getInitialControlCommand: clamp the measured steering, so the published command starts within the limit.Before / after. The vehicle bag (14:24:18-14:26:02) replayed into the MPC twice, without and with this PR, plotted together:
mpc_before_after.mp4
More detail: (vehicle bag replayed; right: tire angle and speed. Black = measured, orange = MPC on the car, blue = gate output sent to the car, cyan = MPC with this PR replayed on the same inputs, red = car MPC failing, green = Autoware in control):
Onset + engages 1-2 (32 s):
mpc_latch_onset_engages_1_2.mp4
Engages 3-4 + release at stop (66 s):
mpc_latch_engages_3_4_release.mp4
Related links
Parent Issue:
How was this PR tested?
1. Unit test
MPCTest.OsqpCalculateAfterSyncBeyondSteerLimit(new): sync a command 0.15 rad beyond the limit, then solve.primal infeasible,qp solver error.test_lateral_controllerpass.Controller test
FakeNodeFixture.manual_steering_beyond_limit_does_not_latch(new,autoware_trajectory_follower_node): in manual, the driver steers 0.1 rad beyond the limit with a low-confidence reference, straightens the wheel, then engages; the command must follow the wheel.test_trajectory_follower_nodeand 13 of the PID controller pass.colcon test --packages-select autoware_mpc_lateral_controller --event-handlers console_direct+2. Vehicle bag replay into the trajectory follower alone (open loop, 14:23:16-14:26:14).
Outside the latched span, the replayed command matches the vehicle's within 0.03 deg (before) / 0.1 deg (this PR).
How to reproduce the replay
Workspace: pilot-auto.x2 on
feat/v0.64/e2e(needsautoware_launchfrom autoware_launch.x2 andj6_gen2_description). Build once without and once with this PR.Bag: vehicle drive 2026-10-06, 14:23:16-14:26:16 JST (shared internally). Needed topics:
/planning/trajectory,/localization/kinematic_state,/localization/acceleration,/vehicle/status/steering_status,/system/operation_mode/state.a. Launch the trajectory follower alone (same remaps and params as
tier4_control_launch),controller_replay.launch.xml:b. Play the inputs only.
/system/operation_mode/stateneeds a transient_local QoS override (qos.yaml):Only the inputs are played, so the recorded
/control/trajectory_follower/control_cmdcomes from the replayed controller.c. Check.
/control/trajectory_follower/lateral/diagnosticmessages with emptydata(the error log is throttled): 2993 before, 0 with this PR./control/trajectory_follower/control_cmdlateral.steering_tire_angleinreplay_outwith/vehicle/status/steering_statusfrom ~71 s into the bag (14:24:27): before it stays at 0.6908 rad (39.58 deg) until ~14:25:56; with this PR it follows the measured steering, max 0.6405 rad.Notes for reviewers
m_ctrl_cmd_prevmixes the offset-compensated and published frames./control/command/control_cmd), which follows the measured steering in manual, not the MPC output. Not changed here.Interface changes
None.
Effects on system behavior
After the driver steers beyond the limit in manual, the MPC no longer locks onto an out-of-range command, and engaging no longer steers toward it.