MPPI PathAlignCritic: Add a minimum dist for occupancy check#6091
MPPI PathAlignCritic: Add a minimum dist for occupancy check#6091adivardi wants to merge 11 commits intoros-navigation:mainfrom
Conversation
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
After looking into adding break early when computing the path's arc-length Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
adivardi
force-pushed
the
av/mppi_path_align
branch
from
4883eeb to
6962947
Compare
So basically: don't make it too large? Good tuning advice for the documentation / readme 😉 We have something similar for disabling other critics in the case of occupancy, no? Should we use this there as well? Finally, I'm curious if you had to tune your path follower critic (or modify?) to use a further path point in this case to overtake so nicely. Maybe something to consider for further improvements on this behavior? Something's weird with CI. Can you rebase/pull in main to try again? I've been seeing some outages though on CircleCI lately in the docs, so maybe they're just having trouble in general. |
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
Conceptually sound, just some details :-)
| auto getParam = parameters_handler_->getParamGetter(name_); | ||
| getParam(power_, "cost_power", 1); | ||
| getParam(weight_, "cost_weight", 10.0f); | ||
| getParam(occupancy_check_min_distance_, "occupancy_check_min_distance", 2.0f); |
There was a problem hiding this comment.
min_distance_occupancy_check to match the styling of the other related parameter
| // (e.g. not driving very slow or when first getting bearing w.r.t. the path) | ||
| utils::setPathFurthestPointIfNotSet(data); | ||
| // Up to furthest only, closest path point is always 0 from path handler | ||
| const size_t path_segments_count = *data.furthest_reached_path_point; |
There was a problem hiding this comment.
Please store this to use rather than accessing the *data each iteration, it actually has performance implications.
| // Find integrated arc-length distance along the path = total dist traveled along the path to each | ||
| // path point | ||
| // loop until end of path, to guarantee don't truncate long trajectories when | ||
| // furthest_reached_path_point is small (e.g. when all trajectories curve away from the path) |
There was a problem hiding this comment.
TMI: I think we can remove this for the old comment. Maybe just add to it "... and find path IDX corresponding to the max occ. check distance"
| path_pt = 0u; | ||
| float Tx_m1 = T_x(t, 0); | ||
| float Ty_m1 = T_y(t, 0); | ||
| // At each (strided) traj point, find the path point whose integrated arc-length distance along |
There was a problem hiding this comment.
| // At each (strided) traj point, find the path point whose integrated arc-length distance along | |
| // At each (strided) traj point, find the path point whose integrated distance along |
There was a problem hiding this comment.
I don't like this "arc-length" language, its just an integrated distance, I'm not sure what the "arc" is supposed to be. The integrated distance phrase I think is clear and descriptive of what's happening SUM(path(i) - path(i-1))
| // find the first path point that is further than | ||
| // max(occupancy_check_min_distance_, furthest_reached_path_point) | ||
| if (occupancy_check_distance_idx == path_segments_count && | ||
| path_integrated_distances[i] > occupancy_check_min_distance_ && | ||
| i >= *data.furthest_reached_path_point) | ||
| { | ||
| occupancy_check_distance_idx = i; | ||
| } |
There was a problem hiding this comment.
Can this not simply be if (path_integrated_distances[i] <= max(occupancy_check_min_distance_, furthest_reached_path_point)) {occupancy_check_distance_idx = i;}? It'll just keep iterating and stop once its larger than that distance. So it'll undershoot by 1 path point, I suppose, but you could also just do occupancy_check_distance_idx = i + 1; (just check that i+1 < size else do just i). Probably store the max() before the loop for not recomputing that each iteration though.
I don't see why we need to initialize it to max, nor check on that status occupancy_check_distance_idx == path_segments_count. Seems overcomplicated.
Question: What about offset_from_furthest? Does that not matter? I don't think so, but did you consider it?
Question: Do we need to retune the value for max_path_occupancy_ratio to be larger now since we're checking a much smaller portion of the path?
2 participants
Basic Info
Description of contribution in a few bullet points
The occupancy check is used to disable the critic if a certain percentage of the coming segment of the path is in collision.
Previously, the segment length was defined by
furthest_reached_path_point.This PR adds a minimum distance for the occupancy check in the PathAlignCritic, so if
furthest_reached_path_pointis too short, the occupancy check distance is extended up to the min dist.This solves a behavior of MPPI where in presence of an obstacle straight ahead, the robot slows down instead of overtaking the obstacle. This is because when slowing down,
furthest_reached_path_pointgets shorter, so the PathAlignCritic is never disabled. With this change, even when the robot slows down, the critic is disabled, freeing the controller to plan around the obstacle.The main downside is, if the distance is too big, the critic may be disabled too early, which may cause deviation from path (if for example, the reference point for
PathFollowCriticis beyond a curve on the path).Description of documentation updates required from your changes
Description of how this change was tested
Tested in both simulation and our real robot.
The effect is impressive. Without the change, our robot cannot overtake an obstacle wider than ~1m. With the change, it can overtake obsatcles up to 2m wide easily.
Without the change:
pathalign_nochange_s.mp4
With the change:
pathalign_change_s.mp4
pathalign_change_2m_s.mp4
Future work that may be required in bullet points
I would appreciate advice on setting the default value of this param. For our robot, it is set to 5m. But our robot is big and drives fast. I figured most robots are small, so I set it to 2m.
For Maintainers:
backport-*.