[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.

[SteveMacenski]

This is a pretty niche problem that is going to likely necessitate that you create a few of your own custom plugin critics to score the MPPI samples w.r.t. only the XY to create the kinds of trajectories you seek. I can't say with any certainty for this situation what you precisely need to do or settings to configure. This is something I myself would need to think through, experiment with, see how it goes, and iterate from there looking into the various modules to make sure I covered my bases. I don't think in abstract I can give you a roadmap for this specialized situation without myself simply doing it and having a robot to experiment with.

Sorry 😦

[SteveMacenski]

Closing due to lack of actionability at this moment. If a user would like to implement and contribute this, we would be happy to merge it, however this is not something maintainers plan to do given the niche nature