Fix Path Angle Critic for bi-directionality #3454

doisyg · 2023-03-07T22:58:17Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3351
Primary OS tested on	Ubuntu
Robotic platform tested on	Dexory simulation

Description of contribution in a few bullet points

Draft PR for addressing the asymmetric behavior of the Path Angle Critic between forward and backward motion.
Currently, as the angle calculation for scoring is done between the trajectory poses direction to the goal (on path) and the trajectories yaws, there is an asymmetric scoring between the forward and backward movement as backward yaws are inverted relative to the direction of motion:
https://github.com/ros-planning/navigation2/blob/34f18d9222e33a2fddb2e45653cd5c0ef7b3bb11/nav2_mppi_controller/src/critics/path_angle_critic.cpp#L65-L69

Also, the filter for discarding scoring if the difference between the robot pose and the robot direction to the goal (on path) is below max_angle_to_furthest, is using the yaw of the robot pose, hence never acting if the robot is moving backward:
https://github.com/ros-planning/navigation2/blob/34f18d9222e33a2fddb2e45653cd5c0ef7b3bb11/nav2_mppi_controller/src/critics/path_angle_critic.cpp#L61-L63

This draft PR makes the Path Angle Critics symmetric by using the minimum angle of the +X and -X orientation.

(on a system with over-weighted PathAngleCritic)

Without this PR:

without_angle_fix.mp4

With this PR:

angle_fix.mp4

Maybe we should also discuss if the symmetric bi-directional behavior of the Path Angle Critics should be optional.

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

nav2_mppi_controller/src/critics/path_angle_critic.cpp

SteveMacenski · 2023-03-08T01:33:16Z

nav2_mppi_controller/src/critics/path_angle_critic.cpp

@@ -58,17 +58,26 @@ void PathAngleCritic::score(CriticData & data)
  const float goal_x = xt::view(data.path.x, offseted_idx);
  const float goal_y = xt::view(data.path.y, offseted_idx);

-  if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) {
+  double forward_angle = utils::posePointAngle(data.state.pose.pose, goal_x, goal_y);
+  double backward_angle = abs(angles::normalize_angle(forward_angle + M_PI));


I think we need to make a parameter if multidirectionality is valid. If we have preferences for forward, we should not be skipping based on the path going in the opposite direction of the current orientation. We need to apply it in that case (that's actually the most extreme case of why this critic exists to help push that orientation motion).

See new commit, by using the plan poses orientation info, we can support all use cases transparently:

following in the forward direction a plan which has poses with orientations pointing in the direction of path increment

following in the backward direction a plan which has poses with orientations pointing in the opposite direction of path increment

doing an inversion manoeuvre when starting to follow in the backward direction a plan which has poses with orientations pointing in the direction of path increment

doing an inversion manoeuvre when starting to follow in the forward direction a plan which has poses with orientations pointing in the opposite direction of path increment

Not all planners provide the orientation though - like NavFn or Theta* just set the XY coordinates, not the full SE2 poses (because its impossible for them to - they know nothing of kinematics).

It is a slick solution though if we had that :/ I suppose we could update those 2 planners to use orientations with something like https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/smoother.cpp#L284-L300 (broken out into a nav2_util). Or even protect against users' custom planners having this in the PlannerServer by checking the output of the path and automatically adding the tangent orientations if not set by the algorithm using the util.

doisyg · 2023-03-08T17:04:49Z

Giving that problem a bit more thoughts, I pushed a new commit with a different method that should not need a dedicated parameter to activate or not the symmetric behavior:
The new method makes use of the goal poses yaw to decide which orientation (direction to goal or direction to goal + pi) to use. Tested in practice, it seems to work great. This however assumes that the global path poses are filled with proper orientations, but that's usually the case for modern planner. WDYT ?
This is more a POC for discussion than an exhaustive solution covering every corner cases.

mergify · 2023-03-08T17:07:56Z

@doisyg, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2023-03-09T02:21:17Z

See comment in thread

SteveMacenski · 2023-03-09T02:24:37Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

 {
  double pose_x = pose.position.x;
  double pose_y = pose.position.y;
  double pose_yaw = tf2::getYaw(pose.orientation);

  double yaw = atan2(point_y - pose_y, point_x - pose_x);
+
+  if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) {


Not a bad solution, but again I think this requires explicit parameterization if this is allowable. If you have a robot system with a preference not to reverse if possible, then we don't want to set the angle this way. Else, we should be using the actual orientation. It also feels weird to me that you're hiding this detail into a generic utility which is just supposed to be finding the angle between a point and a pose. This feels wrong to have this logic here, that's not the role of this function.

... Or maybe as long as moving backward is possible I suppose the other critic should take care of that, but it needs to be tested to make sure this doesn't incentivize more backward motion.

The main concern I see is if a user literally has min_vel_x set to 0.0 so it cannot at at all move backward, this would incentivize reversing motions that are not possible at all rather than fully using the force of the purpose of this plugin to make the robot do a full 180 turn to get to its desired forward-facing heading.

So I think a parameter is needed for the situation, at minimum, where reversing is 100% unequivocally impossible by the users' settings. The question I then have is if we should only apply it in that situation or if we should also apply it when we would prefer not to move backwards. I'm just not entirely sure from a quick glance - I'd need to think about it and probably test.

There's definitely reason to make sure that the PathAngle doesn't use the inverted orientation though - when reversing is impossible. When it is possible but is weighted not to do so, that's when I'm not sure what we should do exactly here.

SteveMacenski · 2023-03-09T02:27:43Z

nav2_mppi_controller/src/critics/path_angle_critic.cpp

    return;
  }

  const auto yaws_between_points = xt::atan2(
    goal_y - data.trajectories.y,
    goal_x - data.trajectories.x);
+
+  const auto yaws_between_points_corrected = xt::eval(


I think I made this same comment before, this is doing elementwise selection of which orientation to use by what's the most convenient, that's not a valid application of a critic function I don't believe. I think we have to decide which orientation we're trying to achieve and penalize applicable to that. Else, some trajectories in a set will be penalized by different criteria than others. I have to think about that a little bit though, maybe it is OK to do elementwise but it feels like that needs to be really well thought out, not on a whim.

If we have a system preferring forward, then only forward should be considered. If we have a system where reversing is viable and we're reversing or wanting to reverse, then it makes sense to use the inverted orientation. If we have a system where reversing is viable and we're not reversing or wanting to reverse, then it makes sense to use the real orientation.

I'm thinking it might be OK to do elementwise as long as a trajectory is generally going forward or reverse -- but what happens if we have a backup maneuver where the trajectory legitimately has a portion of the path which is backing up and another section that is moving forward within the time horizon? In that situation, we'd be applying different orientation direction penalties which makes my head hurt in trying to figure out if that's a good idea or not

SteveMacenski · 2023-06-12T23:04:48Z

PR to supersede shortly

PoC

e76f412

doisyg mentioned this pull request Mar 7, 2023

AUTO-645 Fix Path Angle Critics for bi-directionality botsandus/navigation2#12

Merged

doisyg changed the title ~~Fix Path Angle Critics for bi-directionality~~ Fix Path Angle Critic for bi-directionality Mar 7, 2023

abs backward

896d156

SteveMacenski requested changes Mar 8, 2023

View reviewed changes

change bi directional method

46804c5

doisyg marked this pull request as draft March 8, 2023 17:17

SteveMacenski reviewed Mar 9, 2023

View reviewed changes

doisyg mentioned this pull request Jun 6, 2023

[MPPI] Enforce inversion MOD (NO TEST) #3600

Closed

7 tasks

SteveMacenski closed this Jun 12, 2023

SteveMacenski mentioned this pull request Jun 12, 2023

updating mppi's path angle critic for optional bidirectionality #3624

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix Path Angle Critic for bi-directionality #3454

Fix Path Angle Critic for bi-directionality #3454

doisyg commented Mar 7, 2023 •

edited

Loading

SteveMacenski Mar 8, 2023

doisyg Mar 8, 2023

SteveMacenski Mar 9, 2023 •

edited

Loading

doisyg commented Mar 8, 2023 •

edited

Loading

mergify bot commented Mar 8, 2023

SteveMacenski commented Mar 9, 2023

SteveMacenski Mar 9, 2023 •

edited

Loading

SteveMacenski Mar 9, 2023 •

edited

Loading

SteveMacenski Mar 9, 2023

SteveMacenski Mar 9, 2023

SteveMacenski commented Jun 12, 2023

Fix Path Angle Critic for bi-directionality #3454

Fix Path Angle Critic for bi-directionality #3454

Conversation

doisyg commented Mar 7, 2023 • edited Loading

Basic Info

Description of contribution in a few bullet points

For Maintainers:

SteveMacenski Mar 8, 2023

Choose a reason for hiding this comment

doisyg Mar 8, 2023

Choose a reason for hiding this comment

SteveMacenski Mar 9, 2023 • edited Loading

Choose a reason for hiding this comment

doisyg commented Mar 8, 2023 • edited Loading

mergify bot commented Mar 8, 2023

SteveMacenski commented Mar 9, 2023

SteveMacenski Mar 9, 2023 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Mar 9, 2023 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Mar 9, 2023

Choose a reason for hiding this comment

SteveMacenski Mar 9, 2023

Choose a reason for hiding this comment

SteveMacenski commented Jun 12, 2023

doisyg commented Mar 7, 2023 •

edited

Loading

SteveMacenski Mar 9, 2023 •

edited

Loading

doisyg commented Mar 8, 2023 •

edited

Loading

SteveMacenski Mar 9, 2023 •

edited

Loading

SteveMacenski Mar 9, 2023 •

edited

Loading