More smooth controls on hardware

[artofnothingness]

It seems to me that sometimes robot does a little wobbles following the path

[SteveMacenski]

Can you give a virtual example of that?

[artofnothingness]

While following the straight path Robot can wobble slightly, i'll show gifs later. It's ok overall, and barely noticeable but i think it could be better. And end up trajectory probably could be more robust (not jumping every moment)

[SteveMacenski]

In the same theme: feasibility critic or in the apply feasibility constraints function. Make sure that from timestep i to i+1 we limit the randomly generated control to be within the acceleration limits of the vehicle. That should probably also deal with some of the smoothness related issues.

[SteveMacenski]

See comment https://github.com/artofnothingness/mppic/issues/1#issuecomment-1152746137

Also, @padhupradheep did some testing on his robot and noticed:

I tried it on our MPO-700, since the omni-directionality is based on the steered casters, it had a very shaky behavior. You could imagine, the wheels were re-orienting frequently. But that was huge a step. I should first tune the parameters in our Swedish wheeled robots, then once that it gets better, then I could try it on the caster wheeled based omni directional robot. I'll update you.

With that said, the caster-steer is an odd platform, but if that's seeing it, its likely it is happening on just typical differential drive robots too. The caster-steer is an omnidirectional platform, so also shows more in Y (or could also just be not well tuned for prefering forward / anti-twirling). It would still show up on mech omni wheels, though be less immediately obvious

[padhupradheep]

With that said, the caster-steer is an odd platform, but if that's seeing it, its likely it is happening on just typical differential drive robots too.

Yes, but the affects on the caster-steer will be much high.

It would still show up on mech omni wheels, though be less immediately obvious

Exactly! till now, there are no open source local planners, that can be used on caster-based omnidirectional robot (even TEB is not suitable for it from my experience), mainly because of the non-smooth control commands that it produce because of the optimization. With that said, the Swedish wheeled robot for the same control (from my previous experiment) would have a much better motion, since there is no need for a particular mass of your robot to reorient, whilst the robot is moving. Even if there are lot of jerks as part of your control profile, the Swedish wheeled platforms will be bit stable unlike the caster wheel steered robot. Of course, jerks are something that we definitely don't want, but I hope you get the subjective comparison.

[SteveMacenski]

Sounds like we shouldn't be considering powered-casters then right now if they are known not to work even on other controllers that do work for most every platform type. We can always come back to that later, but we should get something out now for the 99% of users and then seperately focus on the bits needed for that edge case (if you're open to helping!).

I'd be interested in a video of the mecanum wheels to see the relatively non-smoothness you refer to. I think some videos would be illustrative for us. It might be worth also experimenting with increasing the number of iterations from 1 to 3-5. I believe that will slow down the update rate a bunch but might give us some clues if that's a direction we should be going for stability.

[padhupradheep]

I'd be interested in a video of the mecanum wheels

Sure!

It might be worth also experimenting with increasing the number of iterations from 1 to 3-5. I believe that will slow down the update rate a bunch but might give us some clues if that's a direction we should be going for stability

I'll also try to make a comparison if possible.

[SteveMacenski]

I also mentioned in another thread (we should move over here) that we should try to test for now with just X motion and rotation to get that down pat before we go too far into Y motions. Anything to do with smoothness we can do in X/$\theta$ then extend to Y. I suspect things might not be as much of an issue in X/$\theta$ space or might be manageable.

For adding Y, then we can work on extending the behavior appropriately to omni platforms to use the best of what they can do. Does that make sense? I'm trying to decouple problems as much as possible so we have a few more attainable objectives: (1) working for differential / X$\theta$, (2) extend to Y for omni specially mecanum wheels or similar, (3) work on potentially additional anti-rotation critics, potentially, for specifically these powered-caster wheels as on odd edge case, but that may be left as later work.

I think for action points:

• Try adding more iterations to the optimization to further refine motions
• Try tuning critics (though from this I think maybe more critics are required)
• Create a 'smoothness' critic based on the last command
• Anything more predictive we can do? Penalize away from current velocity? Or improved modeling? Is it better to run faster? Slower?

Make sure while testing replanning is off so that we don't see jerking due to replanning off course and we're just working on steady state 'follow that path'

[padhupradheep]

I also mentioned in another thread (we should move over here) that we should try to test for now with just X motion and rotation to get that down pat before we go too far into Y motions.

I totally agree with this point. I'll then start my testing with our differential robot and then maybe move on to omni. I'm away for the rest of the week, so I'll start my testing next week.

For adding Y, then we can work on extending the behavior appropriately to omni platforms to use the best of what they can do.

It will be definitely possible to have a base understanding on how the parameters affect the performance of the system.

Create a 'smoothness' critic based on the last command

Low pass filter had done wonders for us in the past, especially for the optimization based algos. Of course, there are other filters as well.

[SteveMacenski]

@padhupradheep see https://github.com/artofnothingness/mppic/issues/66, by default the model_dt is 10hz but we have Nav2 default controller frequency at 20hz. If we shift only 1 control sequence over for the next iteration then that could be contributing to a bad starting point resulting in local instabilities. I'd try reducing model_dt to 0.05 to correspond with 20hz (or whatever rate you're using) and see if that helps!

Maybe also the temperature parameter?

[SteveMacenski]

Other options if going the critic route:

• The original paper has a (V - $V_{desired}$) term to penalize not going to desired velocity. We drive forward along the path instead, but might be worth seeing if that helps? I suppose it doesn't hurt to have that cost function available for users. That is probably though more of another potential feature to add
• Also a cost function from the original paper on ||u|| - which may or may not be helpful for smoothness, but this is a common term for MPC, so I think that's a good choice to try to minimize effort - that should help in jerking.
• ^ but maybe (u - $u_{i-1}$) would do that? That is also a common term for MPC which would directly be penalizing smoothness

This video is also useful https://www.youtube.com/watch?v=1AR2-OHCxsQ they also talk about jerky behavior that they smooth out with a weighted average of trajectory costs -- which I think we emulate with a softmax, which again points to temperature as a parameter of interest as well without changes to critics to potentially smooth out behavior?

[artofnothingness]

Temperature parameter has impact on smoothness, but using it has its cons (weights on each trajectory would mean less). Critic on control diff is what i thought about to implement for smoothness.

I already mention previously desired speed critic. I did implement it in the past instead of goal critic / path follow critic and it worked worse in obstacle intensive environment.

weighted average of trajectory costs -- which I think we emulate with softmax

Softmax was taken from one of their articles

[SteveMacenski]

I already mention previously desired speed critic. I did implement it in the past instead of goal critic / path follow critic and it worked worse in obstacle intensive environment.

I don't think that's something for solving this particular issue. I'm actually thinking about a general desired speed critic to use in place of a path tracking/goal critic. For instance, in that youtube video, an abstract "go X m/s" without any particular path makes the robot go around the track implicitly. That might be a useful feature to have. Right now, we only drive the robot forward when we have a goal or path.

Softmax was taken from one of their articles

:+1: I thought so, just summarizing my findings that it might be useful to play with temperature at least a little bit. The last 2 bullets seem like if we're going to add critics, those are the ones to have. I think the $\Delta U$ critic would do alot, but also minimizing $U$ in general could aid -- plus since pretty much every MPC implementation known to man uses those 2 critics, figure it would be good to have here

(also super excited that GitHub supports LaTeX style math in Markdown now!)

[SteveMacenski]

But to summarize, because there's alot now scattered across a few comments. Things we should try / add:

• Tune: Play with model_dt, forward number of time steps, and iteration count to see if there's a relationship with jerkiness to any of them
• Tune: Play with the frequency of MPPI (right now 20hz is what we've been testing with) to see how long it can go before it degrades to unlessness and if we push it up more if the jerking is reduced
• Tune: temperature of softmax
• See if making model_dt = 1/control_frequency as a strict rule helps across a number of frequencies
• $\Delta U$ critic, penalizing change in U
• $U$ minimization critic, penalizing excessive controls
• Other smoothness critic?

[artofnothingness]

I don't think that's something for solving this particular issue. I'm actually thinking about a general desired speed critic to use in place of a path tracking/goal critic.

It's easy to implement, so we can experiment with combination with other critics

(also super excited that GitHub supports LaTeX style math in Markdown now!)

Yeah that's cool

Play with model_dt, forward number of time steps, and iteration count to see if there's a relationship with jerkiness to any of them

I made batch size 500 and it seems to become more smooth

minimization critic, penalizing excessive controls

It's opposite critic for desired speed. Do we actually need this ?

[SteveMacenski]

I made batch size 500 and it seems to become more smooth

Also worth noting that the video I linked to above mention they run 2560 and 1 iteration. But they're also using the GPU for parallel scoring the trajectories explicitly with cuda. We have the tensor library instead that would do that for us (or swap out for something GPU-able). No need to do anything wild like that right now, just noting their batch size. Maybe increasing this significantly would be worth trying (in the ~1,000 range).

Maybe we need just a bunch more granular options

It's opposite critic for desired speed.

Not quite, its minimizing the control effort applied -- which I suppose in this formation is the same as speed, you're right. I need to think about it more for our case, but my intuition says you're right.

[padhupradheep]

sorry for hijacking the convo.. But tested the controller on a differential drive robot.. (this is before #70 though)

https://user-images.githubusercontent.com/20242192/175245137-4bfdce82-aa11-4c65-85f2-6435ef57a9b9.MOV

[SteveMacenski]

Totally in the conversation! Definitely a bit jerky, but gives us a good starting point for differential drive robots. This comment kind of outlines I think what the steps forward are. If you had an afternoon that you could play with some dials to see if they help (e.g. first 4) that would be fantastic. @artofnothingness and are are working through the critics (last 3) and optimizing performance elements. I'm hoping to have a block of time this afternoon or tomorrow to do some optimization work.

[padhupradheep]

If you had an afternoon that you could play with some dials to see if they help (e.g. first 4) that would be fantastic.

Sure, I have time next week to do that. Currently I'm just stuck in #71 ! I might move to a swedish wheeled robot for the further tests. (of course without the Y-vel)

[SteveMacenski]

Would reducing the weight like @artofnothingness suggests not help if its an overflow issue? I'm kind of surprised the costs we're talking about are that high that a double overflow is possible. Maybe we need to scale things down? I thought that most of them used xt::mean() to get a single value per trajectory and those should generally be small numbers (maybe in the hundreds, at most). Even if in the thousands, ~10 critics should get no where near that boundary

[SteveMacenski]

I did some simulation analysis this afternoon and this is what I found. These are all rough approximate numbers to derive trends to help in @padhupradheep's testing on hardware to be more efficient in use of time to see what we need to do to get it more smooth on the hardware. Note that none of these included the new smoothing critic, I wanted to see what we could do without it.

• If decreasing temperature, the behavior gets alot more jerky, unsurprisingly. If I increase it from the default of 0.25, things appear to me to be more stable, but I have a hard time deciphering if its enough like "this is the solution" or just a component we should consider in addition to another critic. I tested values like 0.5 and was pretty happy with the changes. Testing 0.75 yielded rather sluggish behavior, so I think nothing there or above is good. More looking should be done where in the range of 0.25-0.7 is a good trade off.
• I increased the frequency from the starting point of 20 hz in really rough approximate numbers to get trends. I see with 40 hz that the behavior actually improves. I'm not sure if its any more stable -- its absolutely not any worse -- but I see really very much improved behaviors with sharper turns and backout maneuvers that I would really like to adopt. I think this is like a proxy instead of increasing the number of iteration_count per iteration and instead just executing the controller more frequently. This gives us more chances to apply perterbations to converge to a better solution faster as the robot is executing.
• There definitely is such thing as simulating too far ahead of time. If we increase the time ahead we look excessively, it'll find points on the path around obstacles going down different path solutions than that as found by the path planner as the route that we'd like our robot to follow. 6s (time_steps * model_dt) is definitely too much, but I'm not sure where between 3s and 6s that boundary is.
• For a given time to project forward (time_steps * model_dt), increasing the density of points to consider increases the stability of the behavior and is less wobbly. Also not a surprising conclusion if you think about it - a random extraneous control has less impact. I tested for example with 3s since that's what the defaults are right now (30 time steps at 0.1s increments --> 100 timesteps at 0.03s increments). That implies that someone should first find the optimal time forward to project, then decide how to break that up into timesteps to get optimal stability in behavior
• More iterations (iteration_count) does NOT correlate to improved stability, in fact with 5 iterations per control cycle, things got radically worse.
• Increasing the number of batches considered improves stability

tl;dr

• Higher temperature might be good -- definitely no less
• Higher control frequency needed
• 1 iteration_count needed -- definitely no more
• Potentially want to forward simulate further in time forward, but not in excess of 6s. This may help with more predictive behavior in crowds / busy spaces
• Higher density of points for a particular simulated time forward -- definitely no less
• Potentially more batches to consider per iteration -- definitely no less

I ended my really rough approximate jerking tuning at 40hz, with time steps of 60 for a model_dt of 0.05, with 1 iteration, batch size 400, and a temperature of 0.5.

[artofnothingness]

but I see really very much improved behaviors with sharper turns and backout maneuvers that I would really like to adopt.

Can you show a gif ?

I tested values like 0.5 and was pretty happy with the changes

Backout maneuvers worse with 10-20 hz and 0.5 temperature ?

[artofnothingness]

think this is like a proxy instead of increasing the number of iteration_count per iteration and instead just executing the controller more frequently

Agree with that. more iterations just breaks constraints on given control and make things less stable

Potentially more batches to consider per iteration -- definitely no less

Agree

[SteveMacenski]

Backout maneuvers worse with 10-20 hz and 0.5 temperature ?

These were all with 40hz -- since that's pretty achievable with some of the run-time performance changes. I don't get 10hz to work well with any configurations -- that's too slow for something like this (or even DWB / TEB). I didn't specifically check at 20hz since the behavior I saw at 40hz was so much better looking that I didn't see a reason to go back to any slower

Can you show a gif ?

I'll try to get one this afternoon - but its a relatively intuitive result. If we don't have more iteration_count's to refine the trajectory, having more times via the control_frequency to re-evaluate and improve the control result makes sharp changes converge faster. That's pretty much what I see. It was able to find the optimal trajectory sooner since if it took 10 cycles before to converge to the new path solution, that now happens in 0.25s instead of half of 0.5s, which makes the behavior better when it executes it. Secondary benefits being that since we're executing this at the same time as optimizing for it, that 0.25s puts the robot on a better track after 0.25s to be tracking the change than 0.25s at 20hz where its only half way to converging, thus adding overshoot/undershoot effects since its moving as its optimizing for that 0.25s with momentum.

The test I did was asking the robot to go straight then suddenly passing it a new goal exactly lateral to the robot's current pose while moving. Faster rate made it converge to the new motion of a hard left turn much more cleanly and doesn't have a little "hump" where there was an overshoot while trying to converge to the hard left turn from the previously straight motion

[SteveMacenski]

I think with a temperature of 0.35, model_dt of 0.075, and time_steps of 40 with the otherwise default critics in the README, the controller with 1,000-2,000 batch_size is worth trying at 20hz.

I'd like 40hz, but right now we can't quite do that performance wise. So maybe 1,000 batch_size at 30 hz is worth trying as a middle ground for now @padhupradheep ?

[SteveMacenski]

https://github.com/artofnothingness/mppic/pull/77 Suggested parameters to look at are in this PR. Let us know what you think

[padhupradheep]

Thanks for the update @SteveMacenski !

I'm moving all my testing to an omnidirectional (swedish wheeled) robot which runs humble for further testing. For the start, I'll test everything in the diff-drive mode and later it would be easy for us also to switch to omni mode.

[SteveMacenski]

Changes to the std of angular velocity helped greatly as well (for historical records)

[SteveMacenski]

https://github.com/artofnothingness/mppic/pull/77 completed, along with the complimentary discussion about tuning.