[MPPI] Rotational Constraint

[RobinHeitz]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04 running in Docker Container
• ROS2 Version:
  • Humble
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

My robot's kinematic is basically omni but with the constraint that it cannot, at any time, do any rotations. Therefore, I want to configure MPPI that way that it follows the global plan, without publishing any rotational speed on /cmd_vel. This means, that the poses cannot be reached (rotational wise), if they don't align with the vehicle's base_link frame.

For testing I've used the global planner example straight line, which has poses that align with the base_link.

On planned path execution, the robot moves away from the global path and later aborts. On /cmd_vel I can see it tries to rotate around z.

Based on the following parameters, I would think it does not try to do this:

motion_model: Omni
wz_std: 0
wz_max: 0
wz_min: 0
az_max: 0
az_max: 0

Expected behavior

It can follow the global path.

Actual behavior

Moves away from global path and at some point aborts since it cannot control back.

Additional information

Currently in /odom I'm publishing the actual position based on lidar sensors, therefore this frame is correct.