[Graceful Controller] Fix Incorrect Motion Target Heading output by controller

[SakshayMahna]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5528
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo Simulation
Does this PR contain AI generated software?	Yes
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Due to yaw value discontinuity, graceful controller was giving incorrect motion target heading. This PR fixes the discontinuity, by allowing continuous values of yaw in the direction of robot.

Description of documentation updates required from your changes

No

Description of how this change was tested

Tested multiple times on simulation.

---

Future work that may be required in bullet points

No

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

@SakshayMahna, 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]

Can you explain the logic here? I wonder if there's a more straight forward way... A hand-drawn diagram might be insightful

[mini-1235]

Can I have a general idea of how the motion target heading affects the performance of the Graceful Controller? Then I will know what to focus on other than the visualization issue. At a quick glance, I think it is being used here

navigation2/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp

Lines 66 to 67 in 547820f

	phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight);
	delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight);

(?)

Also, I think the code before the PR is using the orientation from the transformed plan, can we use this orientation for the lookahead point? On second thought, I don't think this works

[SakshayMahna]

The problem was that the calculated yaw using atan2 sometimes came out to be opposite (possibly because of range discontinuity).

We calculate two vectors

• tx: From prev_point to goal_point
• fx: Unit vector from calculated yaw

We take their dot product to determine if they face in the same direction, and adjust the yaw value accordingly (adding pi if negative).

As per what I understand the orientation of the next point is factored into the control equation, and affects the trajectory the robot takes. Here is the paper: https://web.eecs.umich.edu/~kuipers/papers/Park-icra-11.pdf

[SteveMacenski]

Perhaps a dumb question, but isn't the point on the path? Therefore we can simply use:

double yaw = atan2(
    goal_pose_it->pose.position.y - prev_pose_it->pose.position.y,
    goal_pose_it->pose.position.x - prev_pose_it->pose.position.x);

Since the angle should always be the same regardless?

[SakshayMahna]

I can try that, but I think the problem may still persist.

What I understand (correct me if I am wrong), the scenario that @mini-1235 described is taking place due to the output range of atan2 (-pi to pi). At certain points of trajectory the robot yaw, offsets by pi, causing us to see it reversed in the motion direction. Therefore, the dot product calculation may still be required.

[SakshayMahna]

Perhaps a dumb question, but isn't the point on the path? Therefore we can simply use:

double yaw = atan2(
    goal_pose_it->pose.position.y - prev_pose_it->pose.position.y,
    goal_pose_it->pose.position.x - prev_pose_it->pose.position.x);

Since the angle should always be the same regardless?

This is solving the issue. I am not seeing any problems which were pointed out earlier. Yes, it should work (my bad) because in the code I pushed now, we are comparing with goal_pose_it - prev_pose_it.

I will push this.

But I don't fully understand the reason, because point should be between goal_pose_it and prev_pose_it and both the vectors should point in the same direction always.

[mergify]

@SakshayMahna, 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).

[mini-1235]

Sorry for the late response, I was validating the changes here together with #5446, and I also took a deeper look into this problem today

The problem seems to be in these lines:

navigation2/nav2_util/src/controller_utils.cpp

Lines 62 to 83 in d36bdd1

	// Find the first pose which is at a distance greater than the lookahead distance
	// Using distance along the path
	const auto & poses = transformed_plan.poses;
	auto goal_pose_it = poses.begin();
	double d = 0.0;
	bool pose_found = false;
	for (size_t i = 1; i < poses.size(); i++) {
	const auto & prev_pose = poses[i - 1].pose.position;
	const auto & curr_pose = poses[i].pose.position;
	d += std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
	if (d >= lookahead_dist) {
	goal_pose_it = poses.begin() + i;
	pose_found = true;
	break;
	}
	}
	if (!pose_found) {
	goal_pose_it = poses.end();
	}

Where we previously have:

auto goal_pose_it = std::find_if(
    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
      return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
    });

If I revert these lines, then atan(point-prev) also works

I throw a log when the yaw jumps and points to the wrong direction:

[component_container_isolated-10] lookahead dist1
[component_container_isolated-10] Prev pose: (1.00195, -0.0488911)
[component_container_isolated-10] Goal pose: (1.07273, -0.0490915)
[component_container_isolated-10] Intersection point: (0.998805, -0.0488822)
[component_container_isolated-10] yaw3.13876

I tried to plot this:

This assumption seems to fail in this case

navigation2/nav2_util/src/controller_utils.cpp

Lines 116 to 120 in d36bdd1

	// Find the point on the line segment between the two poses
	// that is exactly the lookahead distance away from the robot pose (the origin)
	// This can be found with a closed form for the intersection of a segment and a circle
	// Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
	// and goal_pose is guaranteed to be outside the circle.

I also took a look on #5003, but I could not find the discussion for these lines changed

[SteveMacenski]

Ah, so this fails because the lookahead distance is no longer computed via the L2 distance but now integrated distance. That is subtle. This probably means that we're getting the incorrect exact lookahead distanced point in both use cases, yes?

[mini-1235]

Yes, as shown in my log, I think we are having something like this which causes the wrong orientation

[SteveMacenski]

Welp, that's not good. @SakshayMahna what do you think? 🙃

[SakshayMahna]

Makes sense, that's why using the goal point instead of lookahead point corrects the orientation.
The referred code block logically seems correct, but there might be a mistake hidden somewhere.
I'll have a look.

[SakshayMahna]

I think I understand what is happening now.

circleSegmentIntersection calculates an intersection between a circle centred at the origin and a line segment given two points. The earlier code worked fine based on this logic, because the calculation was done using L2 distance. Starting from origin whichever point was lookahead distance ahead was assigned goal_pose_it and using std:prev to get prev_point_it were used to calculate the interpolated / lookahead point.

auto goal_pose_it = std::find_if(
    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
      return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
    });

However, now we are using accumulated path distance, not the straight line L2 distance from the origin. That is why the circleSegmentIntersection function is not compatible as it may not receive the correct radius it requires. Hence, for the accumulated path distance, we require a different interpolation strategy.

[SteveMacenski]

Yup, that's my understanding too! Is this something you're open to contributing? I think this should be relatively straight forward as well - we could do something a bit more straight forward in linear interpolation? @vinnnyr I know this was something your company was interested in for RPP which is now shared. Can you provide some feedback if removing circleSegmentIntersection for another solution would cause you an issue with smoothness?

I would categorize this problem right now as pretty urgent since the current state of the code is broken. If someone wasn't going to make a change this week to fix this issue, I would consider reverting the original PR for enabling the path integral distance until such a time this is addressed (though not my preference!)

[SakshayMahna]

Yes sure, this seems doable, probably over this weekend.
For the current state is the proposed solution, current commit using goal_pose instead of point a valid thing that can be submitted. Or we directly look into the interpolation and work on that?

Or we can retain the circleSegmentIntersection as is for RPP, and have something like interpolationIntersection for the Graceful Controller.

[SteveMacenski]

Yes sure, this seems doable, probably over this weekend.

Great!

For the current state is the proposed solution, current commit using goal_pose instead of point a valid thing that can be submitted. Or we directly look into the interpolation and work on that?

I suppose I'd rather a final solution, but this does help, so there's some value in a temporary stop-gap. If a solution would come early next week, we could wait instead. This isn't just problematic for the orientation but also selects the wrong point as well.

Or we can retain the circleSegmentIntersection as is for RPP, and have something like interpolationIntersection for the Graceful Controller.

I'm not sure how we'd distinguish it for RPP vs Graceful, but that is an option. I don't think there'll be appreciable changes in the behavior so I think we could make it a global update. Also RPP now does integrated path distances too, no? We just don't see the breakage for RPP because it doesn't care about the orientation.

[SakshayMahna]

Yes, RPP also uses integrated path distances now. And RPP does not use orientation so no problem for that.
I'll try the complete solution then.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_util/src/controller_utils.cpp	100.00% <100.00%> (ø)

... and 10 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SakshayMahna]

This is how it looks now. It does look correct now right?

nav2_gracefulbug_fix.mov

As well these are velocity plots of RPP and Graceful Controller respectively. They as well seem fine, I think.

[SteveMacenski]

Teamwork makes the dream work; @mini-1235 does this resolve the issue for you?

[SteveMacenski]

I'm pinging @vinnnyr to test since I think this impacts his work so as a sanity check of a deployed user

[vinnnyr]

Sorry I see this in my inbox but i’ve been out for the past few days.I expect to get back tomorrow so I can take a look then!On Oct 6, 2025, at 16:40, Steve Macenski ***@***.***> wrote:SteveMacenski left a comment (ros-navigation/navigation2#5530) I'm pinging @vinnnyr to test since I think this impacts his work so as a sanity check of a deployed user —Reply to this email directly, view it on GitHub, or unsubscribe.You are receiving this because you were mentioned.Message ID: ***@***.***>

[mini-1235]

@mini-1235 does this resolve the issue for you?

Yes, I tested this with tb4, and it solves my issue

[mini-1235]

Sorry, one last question

When reversing, which heading is correct?

a. Aligned with the robot pose heading

b. Heading defined by the vector from the robot’s pose to the target point

[mergify]

@SakshayMahna, 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).

[vinnnyr]

Replacing the circular interpolation is OK from our perspective conceptually if it no longer makes sense.

[SteveMacenski]

Making sure forward and reversing works with these changes (and the previous ones that this is fixing) is pretty key, I agree

[SakshayMahna]

I checked with the earlier code, and briefly went through the Graceful Controller paper.
The motion target is basically the pose of the robot that it is following at the current moment. Anytime, it is supposed to show the expected pose the robot should be in, at some later point of time, which is the A that you mentioned, Aligned with the robot pose heading.

I checked with the current code and the previous code (before the interpolation) it seems to be working fine. Is it failing in some case? Or anything that I am missing?

[mini-1235]

I am trying feasible planners with graceful controller, here is my config:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    costmap_update_timeout: 1.0
    introspection_mode: "disabled"
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::"
      downsample_costmap: false           # whether or not to downsample the map
      downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      allow_unknown: true                 # allow traveling in unknown space
      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
      max_planning_time: 5.0              # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP"    # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72         # Number of angle bins for search
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      analytic_expansion_max_cost: 200.0  # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: false  #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      minimum_turning_radius: 1.50        # minimum turning radius in m of path / vehicle
      reverse_penalty: 1.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.2           # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.015
      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      debug_visualizations: false         # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      use_quadratic_cost_penalty: False
      downsample_obstacle_heuristic: True
      allow_primitive_interpolation: False
      smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path

      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

FollowPath:
      plugin: "nav2_graceful_controller::GracefulController"
      initial_rotation: False
      allow_backward: True

Before #5003, I can see both A and B when reversing

2025-10-09.02-45-37.mp4

After #5003 and this PR, I can only see A

2025-10-09.02-49-19.mp4

I checked with the earlier code, and briefly went through the Graceful Controller paper.
The motion target is basically the pose of the robot that it is following at the current moment. Anytime, it is supposed to show the expected pose the robot should be in, at some later point of time, which is the A that you mentioned, Aligned with the robot pose heading.

If this is the case, I believe we can go ahead and merge this PR!

[SteveMacenski]

Thanks @SakshayMahna for the follow up and iteration here! Nav2 only works because of great contributors like you and @mini-1235 :-)