[RPP] Jerk limited trajectory generation

[angstrem98]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(#2815)
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(Custom differential drive robot in Webots, TurtleBot 3 Gazebo simulation)

---

Description of contribution in a few bullet points

• Generate jerk limited trajectories using ruckig package
• Provide accurate goal angle control using proportional controller
• Smooth stopping near goal while respecting kinematic limits due to motion profiling

Description of documentation updates required from your changes

• Add new parameters to documentation, remove unused

---

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

[mergify[bot]]

@angstrem98, please properly fill in PR template in the future. @stevemacenski, use this instead.

• [ ] 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

[mergify[bot]]

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

[mergify[bot]]

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

[mergify[bot]]

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

[doisyg]

Thanks for the PR, it seems to generate nice smoothed cmd vel.

However I believe there is a bug with the rotating_ globlal variable. When rotating at the end of a path, it can be left in the true state because the goal checker stops executing computeVelocityCommands and then this code path is never executed (i.e. it is never set back to false): https://github.com/memristor/navigation2/blob/7f1e4e5584bed038cb660ac10166cc9a27b65bf7/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L395-L398

This creates an inconsistent/unrequired rotation at the beginning of the next path.

The rotating_ globlal variable needs to be reseted before each new execution of a path, for instance by adding rotating_ = false; in setPlan

[mergify[bot]]

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

[angstrem98]

Thanks for the PR, it seems to generate nice smoothed cmd vel.

However I believe there is a bug with the rotating_ globlal variable. When rotating at the end of a path, it can be left in the true state because the goal checker stops executing computeVelocityCommands and then this code path is never executed (i.e. it is never set back to false): https://github.com/memristor/navigation2/blob/7f1e4e5584bed038cb660ac10166cc9a27b65bf7/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L395-L398

This creates an inconsistent/unrequired rotation at the beginning of the next path.

The rotating_ globlal variable needs to be reseted before each new execution of a path, for instance by adding rotating_ = false; in setPlan

Thanks for trying it out and pointing an issue. I've pushed a commit that should fix the problem. Besides resetting it in setPlan as you proposed, rotating_ is being reset here too: https://github.com/memristor/navigation2/blob/4eb8c3920d6e3f772e59f20b67556d4c22a0690d/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L333

I'm looking forward to hearing how it works out!

[doisyg]

Your last PR fixes the rotation bug, thx.

[doisyg]

You should probably make your new parameters dynamic similarly to the others

[SteveMacenski]

Is there another proxy you can use rather than keeping some rotating_ state? For instance, in rotation, check some value which would indicate as such or be able to reset another variable to indicate that?

[SteveMacenski]

Another question is whether its strictly required to have the angular velocity be set with profiles. I think it depends on the aims and goals of this. If the angular velocity is smoothed out too much or parameters not set properly, it could deviate from the path and that will cause substantial oscillatory behaviors. Linear velocity its more essentially clear the value and need for

[mergify[bot]]

This pull request is in conflict. Could you fix it @angstrem98?

[doisyg]

@angstrem98, this PR is promising, any update ?

[angstrem98]

@angstrem98, this PR is promising, any update ?

@doisyg Sorry for the delay, I've been busy with other projects. I think I will be able to finish this PR in the next few days and answer the comments above.

[Aposhian]

I haven't done rigorous testing, but I have tested this in simulation with our robot and found good results!

[Aposhian]

Another question is whether its strictly required to have the angular velocity be set with profiles. I think it depends on the aims and goals of this. If the angular velocity is smoothed out too much or parameters not set properly, it could deviate from the path and that will cause substantial oscillatory behaviors. Linear velocity its more essentially clear the value and need for

As a side note, the recently added RPP lookahead point interpolation(#2872) greatly helps to smooth out angular velocity, as well as linear velocity when scaling is applied around turns. In my experimentation with this PR, I have observed some oscillations when the max_angular_jerk is aggressively small. However, I still think it is a good knob to provide, to complement interpolation.

[angstrem98]

Another question is whether its strictly required to have the angular velocity be set with profiles. I think it depends on the aims and goals of this. If the angular velocity is smoothed out too much or parameters not set properly, it could deviate from the path and that will cause substantial oscillatory behaviors. Linear velocity its more essentially clear the value and need for

I would say that angular velocity still needs profiles (especially when rotating towards heading in position mode.) I don't think about it only as a smoother, but also as a limiter. Imagine a robot without payload heading to pick up some lab samples. While empty it can go fast towards the goal. After picking up the samples, user sets limits that need to be respected in order to bring them safely.

@Aposhian Yes, making jerk limits too low will make robot oscillate for sure, that way you are making it really "lazy". Also concerning @SteveMacenski question: My stance is that if kinematic limits make robot oscillate or deviate from path, RPP regulated parameters are simply not properly set (or limits are unreasonable). User has entered the limits that need to be respected, and they are being respected, but regulated parameters need some tuning for proper performance.

[angstrem98]

Is there another proxy you can use rather than keeping some rotating_ state? For instance, in rotation, check some value which would indicate as such or be able to reset another variable to indicate that?

I got rid of it in the latest push. I am able to check if angle profile is in position or velocity mode.

@doisyg Dynamic parameters should be working now.

[SteveMacenski]

Architecturally, this PR needs work, there are large chunks of code added to existing functions that should be broken out into semantically meaningful chunks and put into well named functions so the code is better self documenting and easier for users to understand.

There are implicit and then explicit return types from modifications of the functions which is really not great. This overall needs alot of work to redesign these changes to be more easily understood and function's not being modified outside the scope of their specific role

[SteveMacenski]

I would say that angular velocity still needs profiles (especially when rotating towards heading in position mode.) I don't think about it only as a smoother, but also as a limiter. Imagine a robot without payload heading to pick up some lab samples. While empty it can go fast towards the goal. After picking up the samples, user sets limits that need to be respected in order to bring them safely.

I agree in pure rotations having some control for smooth behavior is nice. However, for the path tracking PP algorithm part, it should be the one dictating the angular velocity based on the curvature of the path. The 'Regulation' part is only on the linear velocities. If you deviate from the angular velocities, this is no longer a PP and it will not be tracking the path accurately.

We actually used to have kinematic limits in place until https://github.com/ros-planning/navigation2/issues/2621 which brought up an issue there and we removed it in https://github.com/ros-planning/navigation2/pull/2631. @dpm-seasony @vinnnyr can you comment on this PR / thoughts?

Perhaps some of this work doesn't need to be done in RPP itself but in the velocity smoother https://github.com/ros-planning/navigation2/issues/2633 that interacts at the lower-levels that can be tuned or used as appropriate for a given user. Certain elements of this PR I think could stay in that situation, but it would simplify the constraints that we needed to apply.

Yes, making jerk limits too low will make robot oscillate for sure, that way you are making it really "lazy". Also concerning @SteveMacenski question: My stance is that if kinematic limits make robot oscillate or deviate from path, RPP regulated parameters are simply not properly set (or limits are unreasonable). User has entered the limits that need to be respected, and they are being respected, but regulated parameters need some tuning for proper performance.

This needs to be optional to turn off kinematic limiting so that users that don't wish to deeply model their platform can use this without issue.

[Aposhian]

Perhaps some of this work doesn't need to be done in RPP itself but in the velocity smoother https://github.com/ros-planning/navigation2/issues/2633 that interacts at the lower-levels that can be tuned or used as appropriate for a given user.

While I think there is a place for a velocity smoother, a velocity smoother cannot intelligently produce planned velocities to stop at the end of the path.

[doisyg]

Perhaps some of this work doesn't need to be done in RPP itself but in the velocity smoother #2633 that interacts at the lower-levels that can be tuned or used as appropriate for a given user.

While I think there is a place for a velocity smoother, a velocity smoother cannot intelligently produce planned velocities to stop at the end of the path.

I agree. This all comes down to not overshooting a goal. Safe velocities and accelerations/decelerations can be enforced by a velocity smoother (whereas it should be part of nav2 or a lower level node is another debate), BUT the controller needs to be aware of these kinematic constraints otherwise it will generate infeasible cmd_vel which will lead to sub-optimal or impossible control. The example of a robot with a very slow deceleration capability illustrate the problem well: there will be a larger goal overshoot if the controller doesn't start to deccelerate sufficiently before the goal.

[vinnnyr]

I don't think this change will reintroduce the problems we encountered in #2621 since as far as I can tell, this change does not account for the robots actual speed, it just provides limits on the commanded speed.

Problems we ran into in the earlier iteration of kinematic limiting in RPP were related to our robot not having great velocity control when the velocities were relatively small. This is a problem I describe in the velocity smoothing ticket here (see Deadband section): https://github.com/ros-planning/navigation2/issues/2633#issuecomment-1030088443

We actually used to have kinematic limits in place until https://github.com/ros-planning/navigation2/issues/2621 which brought up an issue there and we removed it in https://github.com/ros-planning/navigation2/pull/2631. @dpm-seasony @vinnnyr can you comment on this PR / thoughts?

As far as the "where should this live" I don't really have a strong preference as it relates to our use case. If there is a desire to put it all in the separate smoother, I can imagine a version of the proposed smoother where the controller calls a smoother service to prevent overshoot.

Not sure if that obfuscates the meaning of a smoother though, so perhaps the "prevent overshoot" portion should remain in RPP / controller and the jerk limit + ang vel limits could remain in the vel smoother?

[angstrem98]

This needs to be optional to turn off kinematic limiting so that users that don't wish to deeply model their platform can use this without issue.

This is a very good idea.

Not sure if that obfuscates the meaning of a smoother though, so perhaps the "prevent overshoot" portion should remain in RPP / controller and the jerk limit + ang vel limits could remain in the vel smoother?

I'd definitely keep "prevent overshoot" behavior inside the controller, also the motion profile when turning to heading. When it comes to angular velocity / accel / jerk limits, I guess limiting them inside the controller or inside the velocity smoother leads to same behavior? I'm leaning a bit towards keeping all the limits inside the controller since user can set them in one location. As far as I know, DWB controller also has all the limits inside it?

If you want to move angular limit to velocity smoother and keep it only inside rotate to heading, sure we can do it if it fits user needs the most.

[SteveMacenski]

I'd definitely keep "prevent overshoot" behavior inside the controller

Agreed, the slowing on the approach is the "meat" of this PR in my eyes .

Maybe it all can / should stay, but I think maybe that's better to reassess after this has a little more design work done on it so its easier to read / understand / broken out into functional chunks.

On that front:

• Functions with explicit returns should not also have implicit reference returns as well from these modifications
• I think each of the "chunks" added doing the profiling need to be broken out into functions of a meaningful name so they're self documenting and not simply inline
• The pure rotation stuff should only be done in 1 place rotateToHeading but I'm perfectly happy if you updated that implementation to use the kp stuff for a smoother motion

[mergify[bot]]

This pull request is in conflict. Could you fix it @angstrem98?

[mergify[bot]]

This pull request is in conflict. Could you fix it @angstrem98?