[MPPI] Incorrect Sign Handling for wz Adjustment of Ackermann MotionModel, when vx is Negative in applyConstraints

[hmojtaba]

There is a potential issue in the Ackermann MotionModel's applyConstraints function when adjusting the angular velocity (wz) based on the linear velocity (vx) to enforce a minimum turning radius (min_turning_r_).

Currently, the code is as follows:

auto & vx = control_sequence.vx;
auto & wz = control_sequence.wz;
auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_);
view = xt::sign(wz) * vx / min_turning_r_;

Problem:
The line view = xt::sign(wz) * vx / min_turning_r_; does not handle cases where vx (the linear velocity) is negative properly.
If vx is negative, the adjusted wz may end up with an incorrect sign, resulting in a contradiction between the intended turning direction and the angular velocity.

Example:
When vx is negative and wz is positive (indicating a counterclockwise turn), the result of the adjustment would incorrectly make wz negative, which would imply a clockwise turn.

Proposed Solution:
The adjustment should use the absolute value of vx to ensure that the angular velocity wz maintains its intended direction. The corrected line of code should be:

view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_;

This adjustment will correctly enforce the minimum turning radius constraint while ensuring the angular velocity aligns with its original sign.

Action:
Please update the code to use xt::fabs(vx) in the adjustment for wz to ensure correct handling of the angular velocity when vx is negative.

[SteveMacenski]

Sounds good, please submit a PR! Did you test this solution fixes an issue for you?

[SteveMacenski]

I'm drafting up a PR now to fix this with a unit test - I'd appreciate if you tested my branch just to validate that it fixes your issue #4664

[hmojtaba]

ok, thanks for creating the PR, I currently cannot test this repo, I only noticed this bug while trying to replicate the Ackermann Model on a different test project.