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.

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.

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

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.

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

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