Oblique trajectory instead of a straight trajectory on a holonomic robot

[MNLucas]

Hi all,

Context and Issue

We are working with _teb_localplanner on a holonomic robot.

We have an issue with the planning of forward trajectories because the planner always gets an oblique trajectory (i.e. the robot rotates 45º, then it moves obliquely, and at the end, it straightens up again according to the final orientation). This video shows the robot behavior and it can help you to understand better the situation.

This behavior makes sense since the vel/acc limitations are set for x and y axis (and theta also), so the optimizer is obtaining that the robot can move forward obliquely faster than moving straight ahead.

Question

How can we get trajectories no-oblique when the target is straight ahead?

---

Thanks in advance!

[croesmann]

hey, I am not working with holonomic robots right now, so it is very interesting to see this behavior. And yeah, that's definitely the time-optimal solution as long as the goal is far away enough such that accelerating and decelerating is worth it. Probably, the way to go is to change the optimization problem. However, the main objective is still time-optimality. So one can think of adding penalties for strafing or rotating (however, adding new penalty terms does usually not solve the problem completely, but only shifts resp. relaxes the problem, and it is harder to tune all the parameters.). On the other hand, if we would add a further limitation for the superposition of the velocities, in particular sqrt(vx^2 + vy^2), the optimizer should prefer the direct way since rotating by 45° and back requires extra time. What do you think?

Edit: I watched the video again, and I recognized that the robot is rotating while translating which makes sense. So adding the extra limitation leads to a non-empty nullspace if I am not mistaken. We might still need to add some regularization term, e.g. slightly penalizing strafing or rotation, depending on the application.

[MNLucas]

Great, thanks for the prompt response!

We thought to add an extra limitation for absolute velocity, as you said, this could be sqrt(vx^2 + vy^2).

We have never used g2o framework for optimization purpose. Could you help us with a suggestion about how to add this limitation? We would like to implement and test this improvement for holonomic robots. So, a suggestion would be very helpful for us.

Thank you very much!

[croesmann]

I added this to you in branch omni_abs_vel. You might have a look at the diff in order to investigate what I did to add the new penalty term for further changes.

I can only try this barely in my test simulation roslaunch teb_local_planner_tutorials robot_omnidir_in_stage.launch. So please test this in your setup. If further cost functions are required, feel free to add them and let me know.

[MNLucas]

Awesome, @croesmann!

Thank you so much!

We're going to work on this over the next week. We'll let you know the result of these tests asap.

[pingplug]

hypot(vx, vy) is good for onmi wheel, but it is not good enough for mecanum wheel(abs(vx) + abs(vy) + abs(omega))

[pingplug]

I have idea, it seems work (better than omni_abs_vel branch). But for some reason I can't tell now... I'll make a pull request in three weeks.

[pingplug]

@MNLucas you can try PR #112 with onmi_type=2 (the type you are using in the video) if you don't want oblique trajectory, you can try omni_type=0

[corot]

Fixed with #346