[MPPI] Incorrect Sign Handling for wz Adjustment of Ackermann MotionModel, when vx is Negative in applyConstraints #4661
Description
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.