Optimizer::prepare() was changed by #6072 (https://opennav.org/news/mppi-low-acceleration/)
|
void Optimizer::prepare( |
|
const geometry_msgs::msg::PoseStamped & robot_pose, |
|
const geometry_msgs::msg::Twist & robot_speed, |
|
const nav_msgs::msg::Path & plan, |
|
const geometry_msgs::msg::Pose & goal, |
|
nav2_core::GoalChecker * goal_checker) |
|
{ |
|
if (settings_.open_loop) { |
|
state_.speed = last_command_vel_; |
|
} else { |
|
// Predict state one controller_period forward toward the last command to compensate |
|
// for the latency between measurement and when this command will take effect. |
|
// Clamp to physically achievable range so prediction never exceeds dynamics. |
|
const auto & c = settings_.constraints; |
|
const double dt = settings_.controller_period; |
|
state_.speed = robot_speed; |
|
state_.speed.linear.x = std::clamp( |
|
last_command_vel_.linear.x, |
|
robot_speed.linear.x + dt * c.ax_min, |
|
robot_speed.linear.x + dt * c.ax_max); |
|
state_.speed.angular.z = std::clamp( |
|
last_command_vel_.angular.z, |
|
robot_speed.angular.z - dt * c.az_max, |
|
robot_speed.angular.z + dt * c.az_max); |
|
if (isHolonomic()) { |
|
state_.speed.linear.y = std::clamp( |
|
last_command_vel_.linear.y, |
|
robot_speed.linear.y + dt * c.ay_min, |
|
robot_speed.linear.y + dt * c.ay_max); |
|
} |
|
} |
|
|
|
state_.pose = robot_pose; |
|
state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); |
I think there are two related issues with the implementation, and I'd like to check whether they're known/intentional trade-offs or simply my wrong understandin.
- The "closed loop" branch behaves like open-loop, both under nominal tracking and under disturbance
std::clamp(value, lo, hi) returns value unchanged unless it falls outside [lo, hi]. Since last_command_vel_ was itself produced subject to the same accel constraints, it will normally already sit inside [robot_speed + dt*ax_min, robot_speed + dt*ax_max]. In that (typical) case state_.speed resolves to exactly last_command_vel_, robot_speed is never actually used, only referenced as a clamp boundary. So under normal tracking, the "closed loop" branch and open_loop=true produce the same result. That's a behavior change from before #6072, where the non-open-loop branch was simply state_.speed = robot_speed, pure feedback, no model assumption.
More importantly, even when the clamp does bind, i.e. real feedback disagrees enough with the model to matter ig, it still doesn't recover genuine closed-loop behavior. last_command_vel_ is set from the raw twist MPPI computed, before anything downstream (Collision Monitor, velocity smoother, or an unmodeled disturbance) can clamp or override it:
Let's say the a_min = -.5, a_max = +0.5, dt = 1 sec, robot_speed = 0.5,
Lets say At current time t . v_cmd = 1m/s ; then last_cmd = 1m/s.
suppose a downstream safety layer holds the robot at v_safe (e.g. clamps a commanded 1.0 m/s down to 0.5 m/s) for a sustained period.
In next time step t+controller_freq ( when we call the mppi controller next time ) .
robot_speed = clamp(last_cmd, robot_speed + 1a_min, robot_speed + 1a_max); robot_speed = clamp(1, 0.5 - 10.5, 0.5 + 10.5) ; robot_speed = 1 m/sec. Whole optimization will be based on 1 m/sec rather than 0.5 m/sec. This will not be fixed in further iterations/ successive calls, there will always be this bias.
state_.pose isn't advanced to match the latency-shifted state_.speed (temporal inconsistency)
I think is a more of an inconsistency rather than a trade-off with current implementation. state_.pose is set unconditionally from the raw measured robot_pose, with no corresponding dt-shift.
With shift_control_sequence=true (the default whenever model_dt == controller_period), control_sequence_.vx(0) is pinned directly to state_.speed, i.e. index 0's velocity is the latency-predicted value, semantically "the velocity once the new command takes effect, one controller_period from now", but it gets integrated starting from state_.pose, which is "position right now, at measurement time," not "position once the new command takes effect."
It seems like the complete version of this latency-compensation idea would dead-reckon state_.pose forward by controller_period as well (e.g. via the same integration step, using robot_speed or the state_.speed or average ? over that short window), rather than advancing velocity alone while holding position frozen at "now."
Optimizer::prepare()was changed by #6072 (https://opennav.org/news/mppi-low-acceleration/)navigation2/nav2_mppi_controller/src/optimizer.cpp
Lines 297 to 330 in 4c7cc0f
I think there are two related issues with the implementation, and I'd like to check whether they're known/intentional trade-offs or simply my wrong understandin.
std::clamp(value, lo, hi)returnsvalueunchanged unless it falls outside[lo, hi]. Sincelast_command_vel_was itself produced subject to the same accel constraints, it will normally already sit inside[robot_speed + dt*ax_min, robot_speed + dt*ax_max]. In that (typical) casestate_.speedresolves to exactlylast_command_vel_,robot_speedis never actually used, only referenced as a clamp boundary. So under normal tracking, the "closed loop" branch andopen_loop=trueproduce the same result. That's a behavior change from before #6072, where the non-open-loop branch was simplystate_.speed = robot_speed, pure feedback, no model assumption.More importantly, even when the clamp does bind, i.e. real feedback disagrees enough with the model to matter ig, it still doesn't recover genuine closed-loop behavior.
last_command_vel_is set from the raw twist MPPI computed, before anything downstream (Collision Monitor, velocity smoother, or an unmodeled disturbance) can clamp or override it:Let's say the a_min = -.5, a_max = +0.5, dt = 1 sec, robot_speed = 0.5,
Lets say At current time t . v_cmd = 1m/s ; then last_cmd = 1m/s.
suppose a downstream safety layer holds the robot at
v_safe(e.g. clamps a commanded 1.0 m/s down to 0.5 m/s) for a sustained period.In next time step t+controller_freq ( when we call the mppi controller next time ) .
robot_speed = clamp(last_cmd, robot_speed + 1a_min, robot_speed + 1a_max); robot_speed = clamp(1, 0.5 - 10.5, 0.5 + 10.5) ; robot_speed = 1 m/sec. Whole optimization will be based on 1 m/sec rather than 0.5 m/sec. This will not be fixed in further iterations/ successive calls, there will always be this bias.
state_.poseisn't advanced to match the latency-shiftedstate_.speed(temporal inconsistency)I think is a more of an inconsistency rather than a trade-off with current implementation.
state_.poseis set unconditionally from the raw measuredrobot_pose, with no corresponding dt-shift.With
shift_control_sequence=true(the default whenevermodel_dt == controller_period),control_sequence_.vx(0)is pinned directly tostate_.speed, i.e. index 0's velocity is the latency-predicted value, semantically "the velocity once the new command takes effect, onecontroller_periodfrom now", but it gets integrated starting fromstate_.pose, which is "position right now, at measurement time," not "position once the new command takes effect."It seems like the complete version of this latency-compensation idea would dead-reckon
state_.poseforward bycontroller_periodas well (e.g. via the same integration step, usingrobot_speedor thestate_.speedor average ? over that short window), rather than advancing velocity alone while holding position frozen at "now."