search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.33k stars 1.21k forks source link

[RPP] Jerk limited trajectory generation #2815

Open angstrem98 opened 2 years ago

angstrem98 commented 2 years ago

Feature request

First of all, thanks for an attractive controller. I had a lot of fun using Regulated Pure Pursuit Controller due to it's simplicity to configure and robustness. It used to have acceleration limiting which did not work properly and has been removed. There is some deceleration near goal implemented but it requires large lookahead values in order to prevent harsh deceleration. RPP controller should have motion profile generation with limited acceleration and jerk that allows it to smoothly accelerate from standstill and decelerate when approaching the goal.

Old acceleration limiting was done on current robot speed. Classic approach is to apply kinematic limits to commanded velocities and the underlying hardware tracks the reference. This is being done in the PR.

I'm already working on this feature and opening this issue as part of the procedure to open a PR as noted in navigation2 documentation.

Issue #2633 is closely related, but it is an external velocity smoother. We need to know when to start braking inside the controller in order to reach the goal accurately.

angstrem98 commented 2 years ago

Comparing before and after results.
Sending custom differential drive robot in Webots from point A to point B with original implementation results in commanded linear velocity as shown on image

Before:

original_rl

New implementation in the linked Pull Request produced following commanded linear velocity:

After:

jerk_limited_rl

We can notice that the new commanded velocity has "S" shaped curve - jerk limited.

angstrem98 commented 2 years ago

Motion profile generation is done using ruckig package (it is used inside MoveIt2). Imagine a robot with significant mass driving at moderate/higher speeds towards the goal. We want it to smoothly come to a stop at the goal point. Velocity smoother #2633 could help us to smooth acceleration, but deceleration near goal with current implementation would cause overshoots. Maybe it is not noticeable with TurtleBot which moves at relatively low speeds, but tests with custom robot moving at 0.4 m/s and above showed overshoots with original implementation.

In order to know when to start braking, one ruckig instance works Position mode and tracks remaining path length. While following the path, another instance works in velocity mode and sets the angular velocity command. When we need to rotate to path heading or rotate to goal angle, proportional controller is used to achieve accurate angle control. Tests showed easy achievement of 1 degree tolerance in goal checker.

SteveMacenski commented 2 years ago

I wanted to give you a note that this is still on my review queue. I've been slammed the last few weeks and I'm now getting my footing back to review these bigger PRs that have been sitting around.

The reason this has taken longer is that this introduces a new dependency on a new library I need to review myself. This also has ramifications across the code-base for places we similarly have code that acts to smooth out information (like the odometry smoother, kinematic constraints in both DWB and RPP, etc). For consistency's sake, I want them all implemented the same way so there's some additional work that I need to do to make sure this could be reasonably applied to those situations as well and that I actually understand how this is working under the hood to make sure this is a good technology fit. I believe it is, I mostly need to get up to speed.