[MPPI] Question on Motion model Lag Compensation

[chanhhoang99]

Hi there,
Currently, I am working on carrier robot that carries a heavy load.
I witnessed that when the load increase, MPPI controller motion model seems to not take into account the lag in motor and assume the motor can reach desired command velocity instantaneously, isn't it ?

[SteveMacenski]

all controllers in Nav2 assume that your robot has properly setup robot controllers to be able to execute commands given. If that is not the case, you would need to change your control system to better be able to handle load changes and/or use the new loaded-robot model to change the dynamic characteristics to represent the dynamics of what the robot can accomplish in the loaded state so the controllers can operate on what the robot is actually capable of accomplishing.

Its obviously important that the controllers know about what the robot can actually do.

Your other option is to create your own controller that considers lag and the fact that the robot doesn't actually have the dynamic characteristics you tuned (perhaps tries to estimate these parameters in real-time). Though, its probably easier to either improve your motor control system so that you have better reaction times in the loaded state and/or set the Nav2 controller's dynamic parameters properly for dynamics in the loaded state that require some more time to accelerate / decelerate.

[chanhhoang99]

Hi @SteveMacenski ,
About the option of creating a new controller that consider load changes. How about applying a of filter inside MPPI controller.
The idea is to use first-order lag model in MPPI.
Below are steps that I am thinking of:

1. Uses odometry feedback to estimate motor delay dynamically by comparing last commanded velocities and actual velocities.
2. Applies a first-order lag model in the predict function to make velocity predictions realistic.

About implementation steps:

1. Create a new class that inherits MotionModel and overide predict function()
2. Add a function to the new class that will find a time constant ($\tau$) that varies with load using last commanded and actual velocities every controller frequency cycle.
3. Then when predict() function ticks, the time constant ($\tau$) above will be applied to the control sequence.

How do you think about this ?
Ref: https://val-sagrario.github.io/Dynamics%20of%20First%20Order%20Systems%20for%20game%20devs%20-%20Jan%202020.pdf

I have implemented a prototype new motion model and tested on my real system and seems to work with ~250kg load.
I want to share it with you but my nav2 rolling branch is a little bit behind the main branch( I tried latest main branch but encountered build issue with message_fillter headers).
However, I can verify it by real life observation, we saw that it does not deviate from path with heavy load and robot can maneuver U shape, L shape or rotating around pretty good which it couldn't perform with the same configuration using original MotionModel. Though, it would be great if you can suggest a way to compare and branch mark our motion model to verify the idea.

[SteveMacenski]

Interesting. Yes, I'd be happy to review / merge a PR introducing that feature! I'm a little surprised something that simple works all that well, but I love the K.I.S.S. principle so if you've tested it and it works well, that's enough for me. I'd just like to hear a few more details about what you saw before vs after this change to sanity check in my mind that this works well enough. But, from your desciption it sounds perfect. If you don't want to share those details publicly, I would understand that and you can email me at steve@opennav.org to chat privately.

Can you open a draft PR? I can see about the build issues. I can probably just handle that for you.

I'm curious: do you think this could be applied to other controllers in Nav2 like RPP? That is also common on heavy equipment that would (I imagine) have non-trivial actuation delay. If it can be applied to RPP, then it could be applied to any of them. Perhaps this could even become a shared Controller Utility that is used in the various controllers to abstract the detail

[chanhhoang99]

Hi @SteveMacenski
Could you check the draft here https://github.com/huynhduc9905/navigation2/pull/6/files ?
Please let me know if I should send you some video clips of our testing with this patch.

[SteveMacenski]

@chanhhoang99 I took a look and left some comments. I think we can simplify this more and have the lag compensation feature always available but just default disabled.

Please let me know if I should send you some video clips of our testing with this patch.

Sure! Showing some plots along with that improved path tracking would be nice.

[chanhhoang99]

Hi @SteveMacenski ,
Just sent you an email with bags and videos. Hope you receive it.

[vahapt]

Hi @chanhhoang99
Please correct me if I'm wrong or missing something. That pull request (#5266) seem to be missing. It just changes a few lines to motion model.
The changes proposed here contains many modifications (which should have) but not reflected to the final PR.
https://github.com/huynhduc9905/navigation2/pull/6/files

[chanhhoang99]

Hi @vahapt ,
There are two changes in that PR:

1. Apply the tau to compensate motor lag
2. Unclamp control velocity with acceleration constrain in the predict function

The change of tau seems not to take effect, rather than that, the unclamping control velocity with accel seems to solve an issue when the acceleration is low.(e.g < 0.5 m/s^2 or 0.5 rad/s^2).

For large platform, we want to limit acceleration because it has relation with the torque applying on the motor. The higher the accel, the higher the torque on the motor. When the torque seem to exceeds what the motor can generate to win that inertia force generated when robot is moving or rotating with an acceleration, this also causes the overshoot or delay in motor.

So to tackle this, once we came up with two way

1. Let's the MPPI motion model knows this lag and predict.
2. Let's MPPI motion model knows our driver controller limit max acceleration and predict.

So the lag compensation seems not to takes effect much, but when I remove the acceleration constrain from control velocity and also limit max acceleration that my driver controller allow, MPPI motion model is able to predict that acceleration and control the robot with respect to that accel limit.

[vahapt]

Your PR changes 4 lines of code.