tuning

[SteveMacenski]

Try these out for size. Things to vary for even better behavior from my findings: increase batch size to 2000. Either way @padhupradheep please try increasing the rate to 30 or 40hz (at 1000 if 2000 misses the loop too much) and let me know if higher rate improves performance (and to what level after increasing its no longer all that much more helpful)

[padhupradheep]

soooo.. just tested it on a simple path, no complex turn, could say it as a simple "close to perfectly straight" path. I didn't change any params.

• 30 Hz 1000 batch

-30 Hz 2000 batch

Edit 1

• 40 Hz 1000 batch

• 40 Hz 2000 batch

For 40 Hz, 2000 batch, the average rate went down a bit ~36 hz.

I'll continue to post results.. For now, it will be the same path..

[SteveMacenski]

For reference, the original works on this topic ran at 50hz with 100 timesteps of 0.02s each - with 2560 batch sizes.

It looks like from your results 2000 really does make a big difference. Did you try 3000 or 4000 to see if that trend continues (and where it levels off?)

The runtime frequency when comparing the 2000 batch size experiments look pretty similar. Would you agree from 30->40hz didn't change things much from watching the robot as much as 1000->2000 did? How did these look visually to you on the hardware? Were any of them sufficiently "smooth" for use or still needs more work in that regard? If it helped, by how much?

[padhupradheep]

Yes, I did some experiments. I had some more plots, will share tomorrow. To be short on what I observed,

Would you agree from 30->40hz didn't change things much

1. Yes

things much from watching the robot as much as 1000->2000 did? How did these look visually to you on the hardware?

Definitely yes, it was smooth indeed! But I had some issues with goals close by, the robot was not able to converge, rather was trying to come around and reach the goal. I guess that’s some tuning issues though.

I’m not at the office right now. I’ll give you some more details and answers the rest of the questions tomorrow with the plots

[SteveMacenski]

Yes

Curious if you tried 1000 at 50hz vs 30hz if that mattered at all - there's no way you'll be able to do 2000 at 50hz, so can't test that. I'm more interested in if there's a reason to go any higher than 30hz and if the trend doesn't appear with only 10hz difference. This might be illustrative of a trend of only needing up to 30hz, but just want to validate that with multiple data points before we draw that conclusion.

Excited for the results!

[padhupradheep]

there's no way you'll be able to do 2000 at 50hz,

You are right, it anyways goes down to 30 Hz

Seems 50 Hz with 1000 batches indeed better in terms of smoothness (just saying it in terms of my visual perspective):

but for some reason, the deceleration regions are different.

[padhupradheep]

Were any of them sufficiently "smooth" for use or still needs more work in that regard? If it helped, by how much?

Videos for 50 Hz, 1000 batch size:

https://user-images.githubusercontent.com/20242192/176713014-0139a816-1e27-4e47-870a-c14b5c6de2e1.MOV

I have sent another video in Slack. @SteveMacenski

Give me some more time, I'll try to work on different temperatures, speed, and would definitely would love to tune the critics (especially the goals that are closer, and the robot is going all around it apart from converging).

Some how, I'm not able to get a solid 3 to 4 hours time to work on it together.

Also, I tried to test at 30 Hz, with different batch sizes. For example, using a batch size of 3000, reduced the controller rate to 15 Hz.

[SteveMacenski]

What were the results? Can we keep the thread in this ticket for @artofnothingness to be able to see and keep all our smoothness tuning attempts public so that people later can track it? The videos don't seem to be working, might want to .zip them up and attach the zip file so we can download and see.

@padhupradheep What are the next steps here - do you have suggested changes for this PR for smoothness / path quality or are you still testing things before having some suggested parameters?

@artofnothingness do you think it would help if we reduced the standard deviation of sampling? I would think that would reduce some of the noise (but also could reduce our reachable states - though if we're talking about 1000+, that's much more than we had before and we'll still have outliers). I was also thinking about if we could apply acceleration limits in applyConstraints() in the motion models so that the randomly noised controls were dampened by the achievable accelerations. Do you think that's wise?

[artofnothingness]

@SteveMacenski @padhupradheep Can you try to tweak wz_std ? 0.3 - 1.0 values. With current params i see missing rate on 20 hz on my machine

[artofnothingness]

I was also thinking about if we could apply acceleration limits in applyConstraints() in the motion models so that the randomly noised controls were dampened by the achievable accelerations. Do you think that's wise?

I did such kind of constraints previously and it made things even worse. If I remember it right, i tried to do that twice, and none of those attempts gave me something

[artofnothingness]

do you think it would help if we reduced the standard deviation of sampling

That's a good idea. As far as i remember i made big wz std, when i didn't invent yet path follow critics to get out from local minimums with big goal critic weight

[SteveMacenski]

wz_std: 1.0

That is an exceptionally good idea, I can't believe I missed that. Reducing that to ~0.3 would probably help solve a number of issues. That's a really wild number. @padhupradheep please try reducing that to like 0.5 / 0.3 and let us know if that helps. I suspect that should.

I did such kind of constraints previously and it made things even worse

OK figure worth a shot, thanks for trying

[padhupradheep]

What were the results? Can we keep the thread in this ticket for @artofnothingness to be able to see and keep all our smoothness tuning attempts public so that people later can track it? The videos don't seem to be working, might want to .zip them up and attach the zip file so we can download and see.

Sure, as far as I observed. The controller did pretty well, with a speed of 0.3 m/s. I had the max vel set to 0.5, but the robot never reached it. As far as with smoothness is concerned, the controller performed well with the current set of Params, except for the case of closer goals (Think about a goal right in the rear to the robot), the controller was making the robot to go in clircles before reaching it. So this definitely needs tuning.

Other than that I would like to test the following:

1. Path following with some narrow edges, turns or path! Trying to increase the weight cost for path align constrain (last time when I did it, robot was not smooth for the goals that the robot had to go in the complete opposite direction of the current heading)

2. Higher speed

3. changing the temperature and see, if there is any impact for the above mentioned scenarios that could be better

4. ofcourse the suggestions that you left.

Does this all make sense ?

[SteveMacenski]

I had the max vel set to 0.5, but the robot never reached it...with a speed of 0.3 m/s.

Why is that? Velocity smoother or other things stopping that or did the controller just not achieve the max velocity? I found that I needed to increase the velocity smoother max velocity, if you're using the default Nav2 parameter files for testing.

We definitely were able to tune the path align/follow/angle critics to be more exact path followers, but just letting you know doing that will restrict dynamic obstacle behavior because you're essentially washing out the other non-path critics that would allow it to do so. While I'm not saying these can't be tuned more for hardware, that's probably necessary, I don't want to tune this controller just to be a Pure Pursuit system. I like that this can handle dynamic obstacles and back out of bad situations very fluidly.

Just remember do this with principle so can summarize results and make decisions about parameter changes in the end. Right now, the goal is fixing the non-smooth behavior you mention and generally getting performance improvements. We can always narrow into exact parameters for the critics for path-tracking or obstacle avoidance later, but the goal right now is getting acceptable smoothness of the system in general. My thoughts are to ignore anything related to path tracking quality / critic tuning right now and focus on smoothness topics (unless you think critic tuning would improve smoothness) as the first problem to address.

• Playing with parameters I messed with in this PR for improving smoothness (or others, whatever :laughing: )
• Decreasing the std for angular velocity
• Maybe need a low pass filter on the output
• Maybe need critic for smoothness

Think about a goal right in the rear to the robot

@artofnothingness maybe we need to look at the critics being applied on approach to goal again?

[artofnothingness]

I noticed overall slowness on these params. It could be caused by parameter prune_distance. You increased time steps, but not the path length. Other possible solution is just to increase goal cost/ perfer forward/path follow cost / increase offset of path follow cost

[padhupradheep]

Why is that? Velocity smoother or other things stopping that or did the controller just not achieve the max velocity? I found that I needed to increase the velocity smoother max velocity, if you're using the default Nav2 parameter files for testing.

We are not using the nav2 parameters, we have our own bringup now, which does not (yet) have the velocity smoother.

(unless you think critic tuning would improve smoothness) as the first problem to address.

Of course, that's what I said with the scenario I mentioned regarding a goal to the rear end of the robot.

[padhupradheep]

It could be caused by parameter prune_distance. You increased time steps, but not the path length. Other possible solution is just to increase goal cost/ perfer forward/path follow cost / increase offset of path follow cost

prune_distance along with increasing the goal cost helped me increase the speed of the robot.

[padhupradheep]

tuning_mppic.zip

videos

[SteveMacenski]

Looks pretty good to me! Though the ending is a little jerky, maybe there's something we can do about that. @artofnothingness do you think maybe that's because we're setting N timesteps ahead of time so the points get really dense? Or for some reason we're not handling the end of paths super smoothly in slowing down to the goal? Seems like there could be room to improve there.

@padhupradheep what parameters are those? Having some analysis with your videos would be great :smile: That way we can know what's going on too and work on stuff in parallel

[padhupradheep]

Sorry for leaving the video without the details. As said, the testing was done with the same Parameters as used in this PR. I just changed batch size to 1000 and set controller rate to 50 Hz. Please let me know what else info do you like to have ?

[padhupradheep]

Also, I feel that the simulation of the mpo-500 produces closer results to the real world. If you guys also want to test it, you can give it a go https://github.com/neobotix/neo_simulation2

[artofnothingness]

do you think maybe that's because we're setting N timesteps ahead of time so the points get really dense

That's a good guess

[SteveMacenski]

So where does that leave us? Is there a way to improve behavior on approach?

I'm trying to enumerate the last few things we need to accomplish before a release:

• Smoothness in general
• Smoothness on approach to goal
• Final tuning based on smoothness / hardware testing / behaviors we want the system to provide
• (Potentially) Optimize code further if settings required for final smoothness / tuning are not possible with current execution time

For the most part, there's not a whole lot more I can do at this very moment without some results from hardware evaluations. @artofnothingness could work on smoothness on approach to the goal so its ready for hardware testing, but hardware results are the big blocker to knowing if we need to invest more in some of the optimization elements. I think we're on the edge, so a few of them would be good, but it would be good to measure exactly how much we need to improve by before starting so we know what direction to take (e.g. reduce critics, reduce trajectory generation, reduce algorithmic use, parallelize some elements, etc).

Looking over the main #1 ticket, that seems to cover it as well. We're super close :smile:. By the way, I'll be on PTO from July 24-Aug 17 so I'll be MIA for a bit starting in a few weeks, this is one of the motivating pushes to get something solid before I leave so we can have a bunch more beta testers giving it a whirl during that month so we can accumulate good longer-term results when I'm back to help resolve them.

[padhupradheep]

Quick update, reducing wz_std to 0.1, improves the smoothness significantly. Especially for the goals placed at the direction towards the rear side of the robot, the robot does not oscillate around the vicinity of the goal, rather moves to the goal. More results are to follow.

[padhupradheep]

w_std set to 0.1! And see the smoothness

[artofnothingness]

@padhupradheep did you try 0.3-0.5 ?

[padhupradheep]

I did try it, let me give you the plots! Give me few mins!

[padhupradheep]

I can do 0.5 tomorrow!

0.3

0.4

[SteveMacenski]

What a great discovery!

This value feels like we should back of the envelope around what values are "reasonable" to offer enough variance to evaluating solution paths. To keep in mind, these are std's on distribution of rotational velocities. So 68% of values will be within current_speed +/- std value and 96% within current_speed +/- 2*std, per ~0.05-0.1s timestep.

0.1 given that the translational ones are ~0.3 feels low. Looking at the charts, they also seem very similar for 0.1/0.3/0.4, but its hard to tell with the scale differences (time / Y scale) and without data. @padhupradheep what do you feel? Is there a real difference between 0.1 and 0.3? If not, I say larger is probably safer.

If something much lower than the translational of ~0.3 is better, then I can spend some time testing myself to retune the pairing of values for all the std's for your testing. Maybe even translational at that point could use some tweaking lower. Let me know if you'd like me to do that. There's no reason to make the std's match for both translational and rotational, but the trend probably holds (e.g. if lower rotational is better, its worth trying a lower translational one too).

[padhupradheep]

For me 0.4 seems to be fine on the hardware. As you said, there was not Much difference when observed visually. The only point I wanted to make was, reducing it, helped the smoothness 😃

What a great discovery!

Just applied the pointer you gave in 😅

[SteveMacenski]

Wham bam -- is this smooth enough for your robots after those changes or still more we need to do?

[padhupradheep]

I still have issue with the robot not converging to the goal (not in all scenarios). I still have to tune the goal critics. But I’m more than satisfied with the smoothness, and I guess we all sort have an idea now on the tuning.

[SteveMacenski]

I would think about the on approach to goal behavior separate from the general smoothness issues -- but that is definitely an issue to address, filed a new ticket https://github.com/artofnothingness/mppic/issues/79

Can you summarize the changes to this PR you'd suggest making to merge and resolve smoothness concerns? I can update this PR and do some testing on my side too to sanity check before merging and closing https://github.com/artofnothingness/mppic/issues/41. A video too would be great :smile: I love me some videos

Also curious on your powered casters if this works now?

[padhupradheep]

At the moment, we don’t have a robot with powered casters to be utilised. I hope it should be.

I’ll do the summary tomorrow!

[SteveMacenski]

https://www.veed.io/download/b98c51bc-8ab8-4bb3-9868-b5b9447b9140?source=compressor-sharing Video of testing

[padhupradheep]

For some reason, I'm not able to increase the speed of the vel produced by the controllers, even after increasing the pruning distance. Shall I spin out as a separate issue, I think this PR deals more with smoothing ?

[artofnothingness]

, I'm not able to increase the speed of the vel produced by the controllers,

How do you try to increase the speed ?

[padhupradheep]

I tried to increase the vx_max along with the pruning distance. For the results that you see above, max velocity that was set were 0.5. But, the robot never touched it though.

[SteveMacenski]

I think you mentioned you're not using the velocity smoother, but an easy 'gotcha' if you are and didn't increase those limits.

I tested though with a number of values like 0.5 / 1.0 with a prune distance of 3.0 (just something arbitrarily higher than required) and see consistently not reaching the speed. 0.5 caps out around ~0.35, and 1.0 caps out around ~0.82. Increasing the path follow and goal weights helped a bit (up to 0.4 from 0.5 now), but not a solution-level fix as I thought it might be since that is what drives the robot forward.

Maybe we do need an explicit max velocity critic but with a relatively low weight so that it doesn't make the robot drive at full speed when it should slow down due to collisions and other things. I'm not sure how well that would work out in practice and I definitely don't want the robots just barreling at full speed when there is any reason to slow down (eg. turns, close to obstacles, etc) but definitely worth a try

It might be that since if we're going faster, we're looking further ahead each iteration of a fixed timestep, so its a lower score to go a bit slower since the noise variants at higher speeds will result in worse behavior w.r.t. the existing critics (e.g. odd angles or closer to obstacles). If that's the case, it takes multiple iterations to refine by the existing critics before useful, but before we even get to that point, a slower trajectory is selected as the seed for the next iteration. It might be that if we give the system a small push to go full speed, that it'll break that balance and drive quicker giving more time to refine trajectories further out to be more optimal by the time we get to that timestep physically.

Filed a ticket regarding it https://github.com/artofnothingness/mppic/issues/80 for now, you should set the max speed to something higher than you'd actually like to go and the ratio of ~70-80% of the max speed seems to hold. That way you can test at the speed you'd like while a new critic is developed.

---

Something to note is that on-approach to goal stuff looks much better at higher speeds (but that might just be my eye in simulation). That could be diagnostic.

but I filed a ticket about this so we can address it later and focus on the smoothness results now. I think the speed issue is something we can fix fully using simulation - but it would be good to know how well the smoothness parameters work at higher speeds now, regardless of what the exact speed that represents is.

[padhupradheep]

Filed a ticket regarding it https://github.com/artofnothingness/mppic/issues/80 for now, you should set the max speed to something higher than you'd actually like to go and the ratio of ~70-80% of the max speed seems to hold. That way you can test at the speed you'd like while a new critic is developed.

Got it! Let me give it a try!

[padhupradheep]

Okay, this can be closed now. As you said, I set the max vel to 0.8 m/s and was able to achieve something around 0.57 m/s. Overall the smoothness was good at high speed and even the rotation was fine.

As a summary, I used almost the same parameters from this PR. I changed and experimented with the following parameter to see how the smoothness varies:

• batch size
• controller frequency
• wz_std

Increasing the batch size, indeed helped in the smoothness of the velocity commands generated by the controller. If you scroll down a bit above, you can see that a batch size of 1000 with controller frequency of 50 Hz worked better. Similar result was also achieved whilst having a batch size of 2000 with a frequency of 30 Hz. To further enhance it setting a wz_std to a value between 0.3-0.5 also helps in smoothness. But if for some case, you have a very high sensitive motors and want a coarse smoothness, then reducing wz_std would help you further help the smoothness.

The experiments was carried out in a standard environment with good robot localization and the v_min was set to 0.

If anyone wants to thank after reading the summary, please forward them to @artofnothingness and @SteveMacenski . Because most of the tests were conducted based only upon their pointers.

@SteveMacenski is there something that I miss in the summary further? maybe it's time we merge this PR.

[SteveMacenski]

That's a good summary, thank you. Can you provide me with what you think is a good default for the wz_std? You say 0.3-0.5, can you give me the suggestion for the "best" results you found? I can test from there to sanity check but largely I'm going to take your word on it.

For 1000 batch @ 50hz vs 2000 batch @ 30hz, is there one that you preferred over the other for smoothness characteristics? This is what is particularly hard for me to characterize in simulation so having your thoughts would be valuable. I can test both to see what's realistic given our current compute times, but I'd like to know what's best and work backwards from that if we need to improve performance or what we have now is good enough for great out of box behavior.

[SteveMacenski]

OK, I'll take a look at these this afternoon and come back with an update to the PR with some new values that I've tested

[SteveMacenski]

OK see the updates. I found that 50hz was not quite as good quality in simulation and even with only 1000 batches, we really cannot run at 50hz with that many without some really significant changes in performance. For that reason, I took the 30hz/2000 batches (e.g. can't run this fast; quality didn't look at high)

For 30hz @ 2000 batches it largely works, though maybe every second or two we have an iteration that doesn't quite meet 30hz, usually only off by a millisecond or two. I think that makes sense for us to use this instead and try to get back another 5-10% of performance improvements (#68?) elsewhere to make it run stably.

I pushed it back from 0.075/40 to 0.1/30 for the performance reasons, though it would be nice to push those back down again later. But wouldn't be the end of the world if we didn't.

@padhupradheep what do you think of these?

@artofnothingness do you think we can get some more performance improvements to make this configuration work? I'm on a laptop mobile 8th gen i7 (i7-8565U CPU) which is pretty high power for many robots, though many service robots will have something roughly this powerful. I would figure some people would want more than just this, but I think what we have here is a good initial trade off so making sure there's some wiggle room is important.

[SteveMacenski]

Here's the profile:

callgrind.out.zip

Path align/path angle are some of the highest critics (9% each; path angle is in normalizing angles and path align appears to be about half in the coded logic and half in some internal xtensor stuff), and generating noised trajectories is still about 55% of it (20% integrate state velocities, 9% update state velocities, 21% generate noised controls).

This run didn't include prefer forward critic, that is another good chunk of a critic. Also in normalize angles like path angle critic.

I still think the trajectory generation part is the best to try to optimize since its the most & is directly correlated to the bottleneck of wanting to have trajectories generated quicker to be able to use 50hz or all 2000 samples at 30hz.

[artofnothingness]

@SteveMacenski noise generation basically is a few xtensor calls. we could try generate noises in parallel for vx/wz/vy.

[SteveMacenski]

I wasn't sure if there were perhaps more efficient ways to use xtensor or seeing if we can move stuff under the same function to take advantage of the higher levels of caches. Sometimes shifting things around to keep objects in context can help dramatically.

Trying to generate in parallel might be worthwhile - though it would be good to keep the thread around since spinning up is frequently a major performance bottleneck, but I'm not sure exactly how parallelization libraries handle that kind of thing.

I'm really quite surprised at how long it takes xtensor to sample from a normal distribution or do those strides. I suppose we could go back to a previous question if xtensor is the most efficient option for us and instead try Eigen? With the exception of 1 more critic for #80 and working out what ever is causing #79, this is close to done so that derisks it a bit since we're not planning any major changes from here on.

Maybe we could set up another thread that is running in the background to do some work? For most of the trajectory generation process other than adding the noises to the last control and adding the integrated poses to the state, there's nothing specific about them that requires to be run at that time. We could have a separate thread that runs in parallel to the trajectory evaluation to generate the noised trajectories for the next iteration. It won't double speed, but might give us about 20-30%.

More far-fetched, but perhaps even potentially on initialization we could generate a bunch of noises for each axis and store them in the class. Then each cycle, we randomly sample one from it to use for that given iteration (or maybe even try to offset indices?) - though some relative numbers on memory consumption would tell us if that's practical or not. If we do the offset within the set to create an extra level of randomness, there's no promise that it won't be just as slow as generating a new one, but we couldn't know without trying.

[padhupradheep]

I think for the defaults a batch size of 1000 with controller frequency of 30 Hz should be fine, provided the user uses the velocity smoother along with mppic. If that doesn't help, then they can just increase the batch size to 2000 and increase the controller frequency to 40 or 50 Hz, along with the w_std to maybe as low as 0.2 or 0.1.