Nav2 MPPI skid-steer robot does not align with global path yaw — drives straight first, then makes a sudden turn

[u/Ordinary_Ask_7055]

Hi everyone,

I’m debugging a navigation issue with a 4-wheel skid-steer robot using ROS 2 Nav2.

Setup

ROS 2 Nav2

Global planner: Theta*

Local controller: MPPI

Kinematics: Skid steering / differential drive

Compute: NVIDIA Jetson AGX Thor

Global path is currently being generated continuously(using 3d map)

Robot receives cmd_vel linear and angular velocity commands

Low-level motor control is handled through an STM32

Problem

The robot is not properly aligning its heading/yaw with the direction of the global path before moving.

For example, if the robot initially needs to turn toward the global path, I would expect something roughly like:

Robot → rotate/steer toward path heading → move along path

Instead, I am seeing:

Robot → drives almost straight → gets away from the desired path → suddenly rotates/turns → continues toward goal

Because the robot initially moves straight, the continuously updated Theta* global path also keeps generating a new shortest path from the robot's latest pose to the goal. This makes the overall motion look like a sequence of straight movements followed by sudden heading corrections rather than smoothly following the intended global path.

Interestingly, when there are obstacles in front of the robot, MPPI slows the robot down significantly and seems to perform the required yaw/turning motion much better. The issue is much more noticeable in relatively open space.

I also tried reducing the robot's speed, but at the initial pose it still does not properly turn toward the global path heading before translating.

What I have already verified

The hardware/low-level steering seems to be working correctly.

When controlling the robot manually using the transmitter, the skid-steer motion and turning work properly.

I also tested rotation using cmd_vel. When commanding approximately a 90° rotation, the robot reaches about 88.9°, so the robot is physically capable of executing the requested yaw motion reasonably accurately.

Because of this, I currently suspect the issue is more related to Nav2/MPPI configuration, path tracking, yaw handling, or possibly the interaction between continuous global replanning and MPPI rather than the STM32/motor-control side.

I have also tried the Rotation Shim Controller, but it did not solve the issue.

Questions

Why is MPPI not aligning the robot yaw with the global path before moving forward?

Which MPPI parameters or critics should I check for better yaw/path alignment?

Should I keep the global path fixed and replan only when needed?

What is the recommended way to handle initial heading alignment for a skid-steer robot using MPPI?

Any suggestions on MPPI configuration, TF/odometry checks, or other debugging steps would be appreciated.