search

hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
603 stars 167 forks source link

Toppra does not respect acceleration limits (in C++) #198

Open jess-moss opened 2 years ago

jess-moss commented 2 years ago

Describe the bug I am running toppra on a 10 DoF robot with acceleration and velocity limits in C++, however, toppra is not respecting the acceleration limits.

To see the bug run the following test (which fails due to lack of respect of the acceleration limits): https://gist.github.com/jess-moss/448d293853c08b6357e49c475fef6198

Expected behavior When checking the accelerations and velocities along the toppra path, they should be within the limits created in the acceleration and velocity constraints. However, they are not.

Screenshots I plotted the acceleration of one of the failing joints along with its constraints, which is shown below. Notice how the acceleration does not respect the lower bound. image

In addition, I plotted the position, velocity and acceleration of this joint (see below). Note in this graph, the velocity and position are scaled. I also find it weird how the acceleration jumps a lot for a smooth position and velocity path. image

hungpham2511 commented 2 years ago

Can you check what is the discretizationType of the constraints?

If its Collocation, switching it to Interpolation should resolve the problem.

jess-moss commented 2 years ago

The LinearJointAcceleration was already set to Interpolaton. I set the LinearJointVelocity to Interpolation just to be sure and it did not resolve the problem. Any other thoughts? I have linked the exact test I am running if you want to play around with it. Thanks!

hungpham2511 commented 2 years ago

The LinearJointAcceleration was already set to Interpolaton. I set the LinearJointVelocity to Interpolation just to be sure and it did not resolve the problem. Any other thoughts? I have linked the exact test I am running if you want to play around with it. Thanks!

Thanks. I will try to have a look at the issue shortly.

hitytqsz commented 2 years ago

I encounter to a similar problem like jess-moss: image

hungpham2511 commented 2 years ago

Thanks all for reporting. Hopefully we can find some resource to work on this issue next month. Really sorry for leaving this issue hanging as things have been quite hectic at my work.

mrunaljsarvaiya commented 2 years ago

@hungpham2511 Out of curiosity so I can better understand the algorithm, do you know of any introductory references to understanding the difference between collocation and interpolation discretizations?

hungpham2511 commented 2 years ago

@hungpham2511 Out of curiosity so I can better understand the algorithm, do you know of any introductory references to understanding the difference between collocation and interpolation discretizations?

I think we have a few references in the paper. Have you tried checking?

These are discretization scheme used in ODE integration algorithms.

hungpham2511 commented 2 years ago

Hi,

I had a look (finally, sorry for the long wait). The issue is due to the automatically selected number of points (100) is too small for the path you are giving toppra.

Mathematically when you have a path with high-curvature, a coarse grid points mean there is higher approximation error. This is universal to any algorithms that discretize the problems.

The solution that I am quite happy with is the procedure to automatically generate a grid points, but this only exists in the Python version of the software.

I could spin up quickly such an implementation.

As for @jess-moss issue, you can add this line 

  auto algo = std::make_shared<algorithm::TOPPRA>(constraints, path);
  algo->setN(1000);

You will see that the error become much smaller.

hungpham2511 commented 2 years ago

FYI #201 should address this issue more completely.