add max vel linear

[renan028]

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:

non_max_vel_linear_2.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):

vlc-record-2022-01-11-15h01m21s-non_max_vel_linear.mp4-.mp4

Solution

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

Long path not moving diagonally:

max_vel_linear_2.mp4

Moving around:

max_vel_linear_2.mp4

[corot]

still requires some refinements, but nice fix!

@SteveMacenski, I think this can be interesting also for Nav2 version, so please, take a look if u have time

[corot]

lgtm, but I would remove the L1, L2 param

[corot]

Q

[corot]

cool!

[corot]

There was an old issue for this #89