MPPI acceleration constraints not respected

[tonynajjar]

Bug report

I have a suspicion that the acceleration limits are not working after #5266

Steps to reproduce issue

ax_max: 0.1
ax_min: -0.1

Scenario: some setup that makes your robot accelerate in a straight line

Checkout before and after the PR

Expected behavior

See a curve that roughly respects the acceleration constraints

Actual behavior

Does not seem to respect the acceleration constraints

Reproduction instructions

Additional information

[SteveMacenski]

OK. Now with issues described in:

• [MPPI] Question on Motion model Lag Compensation #5214
• Unclamp noise velocity. #5266

I think we should revert the change and/or make it parameterized about whether to utilize. Clearly this isn't the right answer for everyone as @tonynajjar is reporting issues with acceleration constraints and @adivardi with more wobbling in the angular axis from that change.

@chanhhoang99 I followed up in #5214 for clarification on your tuning adjustments with this change. Can you follow up?

@tonynajjar what are you plotting to get those results you posted in #5266? How would you explain this given we apply acceleration constraints to the output optimal control, regardless of the motion model? That should ensure no matter what (though quality variable) it should meed the dynamic/kinematic limitations. Is IDX=0 somehow jumping illegitimately - I don't see any other way?

[SteveMacenski]

I will go ahead and take on the revert / parameterization action so that this is not delayed. @chanhhoang99 - but I'd love your thoughts.

[tonynajjar]

@tonynajjar what are you plotting to get those results you posted in #5266?

Oops forgot to mention, that's just linear velocity. The path is straight

[SteveMacenski]

But.... what is it 🙃 Position, velocity, Y axis time or motion axis?

[tonynajjar]

Y-axis: cmd_vel.linear.x
X-axis: time

does that answer you?

[chanhhoang99]

@SteveMacenski ,
It's okay to revert it. To be honest, I believe I have not yet tested all the case(we do not have the methodology and deep enough understanding to verify such breaking changes).
About the accel limit constrain is not respect, I believe it's an intended output of the PR.
Imagine we are trying to control a robot with a hard accel constrain(I clamped the velocity on my driver controller at a max = 0.5), so as we control the robot, let's say robot is rotating at 1.0rad/s^2, so it takes 1 second to reach 0rad/s. So we as the controller, we need to stop it sooner than expect.
This behavior, I think matches with the predict function, the MPPI prediction must stop it beforehand like we do with teleop_keyboard. So that's why MPPI produce such a steep velocity, but later it will be clamp by the driver.

[adivardi]

@SteveMacenski Are you also working on a PR to make this parameterized? Or would you like a contribution?
Even with the oscillations, this is quite helpful for our slow vehicle, so it will be great to have an option.