search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.06k stars 550 forks source link

add max vel linear #346

Closed renan028 closed 2 years ago

renan028 commented 2 years ago

Problem Description

When we use holonomic robots, we should limit the maximum linear velocity (L-inf, L-1 or L-2 norm), not only max_vel_x and max_vel_y, because they are coupled.

Otherwise, the optimization for vx and vy results in max_vel_x and max_vel_y in open / collision-free paths. The robot will always move diagonally because the resultant (linear velocity) is greater. However, the maximum velocity is coupled and bounded to the actual maximum linear velocity of the robot. This new constraint should be added to the optimizer.

The video below shows the robot moving diagonally and it fixes the orientation at the end:

https://user-images.githubusercontent.com/6097267/148993227-5773c195-a3c6-4ad6-ab5a-503c104d751b.mp4

And one can see that the behaviour of the robot is not what we expect for a holonomic robot (changing orientation a lot, and moving diagonally):

https://user-images.githubusercontent.com/6097267/148996957-d1a8698e-b858-4eb1-b1a5-319d458dc292.mp4

Solution

Add max_vel_linear for holonomic robots and add the constraint to the optimization.

Long path not moving diagonally:

https://user-images.githubusercontent.com/6097267/148996627-7f598de7-23f8-4c17-818f-28df3a3e92f6.mp4

Moving around:

https://user-images.githubusercontent.com/6097267/148996037-e0d0d8df-6e35-4735-b835-095ed0cad98f.mp4

corot commented 2 years ago

There was an old issue for this #89