Approach to Goal Behavior/Smoothness/Converging

[SteveMacenski]

I think that one might be a little more involved, but its nothing crazy I suspect.

I think we need to not use some critics in close proximity to the goal & make some minimum spacing requirements for the trajectory points, else on approach all 60 time steps or whatever are going to be in the span of a few centimeters, which I suspect is the cause of issues getting right up to the goal. I think we need to make some special rules about on approach to the final target.

But we should find root cause of this behavior, but auditing critics to see what are used on approach to the goal and using only the goal-driving critics would be a good place to start. If its not the critics (unless bug in implementation of goal critics?), then its the trajectories most likely that need some special rules to get to a stop at a pose

[SteveMacenski]

At least the path tracking critics are disabled when in close proximity to the goal:

• https://github.com/artofnothingness/mppic/blob/develop/src/critics/path_align_critic.cpp#L28
• https://github.com/artofnothingness/mppic/blob/develop/src/critics/path_angle_critic.cpp#L33
• https://github.com/artofnothingness/mppic/blob/develop/src/critics/path_follow_critic.cpp#L31

But the goal angle and goal critics are still active til the end to drive towards the final pose.

• https://github.com/artofnothingness/mppic/blob/develop/src/critics/goal_angle_critic.cpp (in fact this only applies when close to the goal -- but maybe needs to be extended? 20cm by default seems low to start applying, that's the goal tolerance in many situations)
• https://github.com/artofnothingness/mppic/blob/develop/src/critics/goal_critic.cpp

Some other critics don't have any restrictions on proximity to goal:

• https://github.com/artofnothingness/mppic/blob/develop/src/critics/twirling_critic.cpp
• https://github.com/artofnothingness/mppic/blob/develop/src/critics/obstacles_critic.cpp
• https://github.com/artofnothingness/mppic/blob/develop/src/critics/prefer_forward_critic.cpp

I think its an easy argument that Obstacle should still be applied in close proximity to the goal -- but potentially prefer forward (move forward optimally) or twirling (dont needlessly rotate about your axis for omni robots) could be removed? Allow the goal to freely define how the robot moves at that point to meet the goal pose within close proximity to the goal.

[SteveMacenski]

20cm by default seems low to start applying the goal angle critic, that's the goal tolerance in many situations. I think threshold_to_consider_goal_angle should be increased quite a bit, maybe 0.5 - 1m or more? That may be a reason that its struggling to converge on the heading angle smoothly, its not able to apply until very, very close

So 2 things to try so far that don't require major changes to the framework:

• Increasing the goal angle proximity for consideration / lowering the weight --> seems to help a bit to diverge from the path to get to the right heading versus get to the goal pose and then rotate to pose in a little less fluid of a way.
• ~Apply a restriction on the prefer forward and twirling critics for close proximity to goal to let it smoothly get to the final pose when in close proximity~ doesn't seem to help

[SteveMacenski]

@artofnothingness I asked @padhupradheep to test on the current state of dev given all the great changes you made to see if that's "sufficient" on the hardware to solve the on-approach to goal smooth convergence issue or give us some feedback about what's happening now to work from. Some of those gains you changed looks to have helped in confined situations for sure.

[SteveMacenski]

I did some playing today while temporarily blocked on the smoothing ticket with this. I have some updates I'll push tomorrow that replaces some of the critics' use of only disabling when in positional tolerance to disabling when within a configurable window around the goal, expanded to 0.4m. I see the successful completion of tasks near difficult goals increase from ~75% success rate to ~90% and a bit more direct in doing so.

[SteveMacenski]

https://github.com/artofnothingness/mppic/pull/95 implements.

@padhupradheep please test this if you can once back from PTO! Excited to hear your experiences and if this is sufficiently good now!

[padhupradheep]

Just give me a week's time, I'll be back in our HQ. Will be one of my top priorities.

[SteveMacenski]

Awesome! Testing #99 would be awesome too, just make sure to not navigate near walls -- its not as good with maintaining distance from obstacles in that PR branch. But, it should be alot smoother velocities and will achieve full speed. Assuming this ticket is resolved and the behavior in the PR branch you like, the last developmental issue is to resolve that distance from obstacles bit -- then its just overhead (documentation / test coverage / fine tuning on a hardware platform) for release. I'm aiming before Thanksgiving (November 24) for release into Nav2.

[SteveMacenski]

99 has confirmed to resolve on hardware