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

[jess-moss]

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.

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.

[hungpham2511]

Can you check what is the discretizationType of the constraints?

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

[jess-moss]

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]

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]

I encounter to a similar problem like jess-moss:

[hungpham2511]

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]

@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]

@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]

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]

FYI #201 should address this issue more completely.