The state of TOPP3

[maxpla3]

Hello Hung,

I've read your paper "On the structure of the time-optimal path parameterization problem with third-order constraints" extensively and tried to implement the trajectory generator. However, it is quite a complex algorithm. I failed at the search for dynamic singularities.

Do you plan to go in that direction in the future and would you provide a reference implementation of TOPP3?

From your perspective, would it be possible to post-process the time scaling (s, ds/dt) within TOPP-RA to at least phenomenologically reduce acceleration discontinuities? What I mean is the following: The time-scaling s(t) from TOPP-RA is not differentiable at certain points (e.g. where forward and backward integration passes meet). Would it make sense to replace those sharp corners e.g. by b-splines? This would not allow for a strict control of the jerk but it would certainly reduce the jerk (in an unpredictable fashion). Although this idea does not represent an ideal solution, maybe it would be good enough for most cases. I'm just wondering whether this heuristic approach would lead to inacceptable consequences, like e.g. exceeding acceleration limits. Do you have any thoughts on that?

Best regards and many thanks for your work! I really like it.

[hungpham2511]

Hi Max,

The TOPP3 algorithm is rather sensitive to numerical errors, much like other shooting approaches to solving time-optimal trajectory planning. I have some lucks with a different approach based on optimization (TOPP-RA) -- this proves to be much more robust than shooting algorithms which TOPP3 is based on.

It is known that when there are 3rd-order constraints, the problem is not convex. This means an efficient and optimal solution is likely hard to find via optimization.

Would it make sense to replace those sharp corners e.g. by b-splines?

Indeed that would be possible. However there will still be difficulty when the squared velocities are small, say, at the beginning and end of a trajectory. Basically while the (s'^2, s) is smooth, it is not the case for s(t) as the transformation between them is highly nonlinear.

Edit: I actually tried that (smooth the velocity profile before calculating q(t)) a few years ago without much success.

[maxpla3]

It is known that when there are 3rd-order constraints, the problem is not convex. This means an efficient and optimal solution is likely hard to find via optimization.

I am totally aware of the complexity of such a task. I have been looking for a decent non-time-optimal trajectory planner for more than a year. I would be perfectly fine with a sub-optimal but jerk-limited solution obtained within less than one second for typical 6-DoF-paths.

However there will still be difficulty when the squared velocities are small, say, at the beginning and end of a trajectory. Basically while the (s'^2, s) is smooth, it is not the case for s(t) as the transformation between them is highly nonlinear.

Yes, there is the square-root which leads to an infinite slope of s' at s'=0. I used an analytical expression derived from a constant jerk motion to bootstrap the integration in the (s',s) plane where s'=0. It is sufficient to use this analytical solution for two or three numerical steps to escape the singularity at s'=0. At such small scales the gained accelerations and velocities are small enough to not violate the constraints. I hope you understand what I mean.

Edit: I actually tried that (smooth the velocity profile before calculating q(t)) a few years ago without much success.

Do you remember the issues due to which you abandoned this approach? Was it primarily the difficulty around s'=0? I can imagine myself working on such a post-processing approach and eventually merging it in this repo if it succeeds.

[hungpham2511]

Do you remember the issues due to which you abandoned this approach? Was it primarily the difficulty around s'=0? I can imagine myself working on such a post-processing approach and eventually merging it in this repo if it succeeds.

It has been quite a while so I don't remember it fully. That said handling s'~0 is one of the issues.

I used an analytical expression derived from a constant jerk motion to bootstrap the integration in the (s',s) plane where s'=0

This sounds quite interesting. Do you have any result yet?

Just want to make sure that I understand the idea, is this correct to say that you will do the following:

• Obtain (s'^2,s) from TOPP-RA
• Smooth the curve (s'^2,s)
• Identify points that can lead to infinite jerks (such as the start and end) and try to calculate a 3-time differentiable function s_smooth(t) that fits the curve (s'^2,s) the most.

[maxpla3]

I used an analytical expression derived from a constant jerk motion to bootstrap the integration in the (s',s) plane where s'=0

This sounds quite interesting. Do you have any result yet?

I did this when I tried to implement TOPP3 and the analytical expression for the first several integration steps worked well. After the bootstrapping I switched to maximum-jerk forward integration. The transition from bootstrapping profile to maximum-jerk profile was not perceivable in the final trajectory.

Just want to make sure that I understand the idea, is this correct to say that you will do the following:

• Obtain (s'^2,s) from TOPP-RA

Yes.

• Smooth the curve (s'^2,s)

Yes. However, I work in the (s',s) plane not (s'^2,s). To be more precise, with s' I mean the time derivative ds(t)/dt.

• Identify points that can lead to infinite jerks (such as the start and end) and try to calculate a 3-time differentiable function s_smooth(t) that fits the curve (s'^2,s) the most.

Yes. Either use some kinde of a spline or the multipe-shooting method from TOPP3.

The final result would still include jumps in the acceleration, e.g. where a switch happens between velocity-limited motion and acceleration-limited motion. But such "constraint switches" usually do not lead to bang-bang jumps in the acceleration and the jerk is therefore smaller anyways.
I would first focus on the smoothing of the boundaries of the motion (s'=0) and on the switching points of maximum-/minimum-acceleration profiles.

If the smoothing is done heuristically using splines (insteand of the multiple-shooting method), it is possible that the smoothed trajectory will violate acceleration and/or velocity limits in/around the smoothed areas.
In this case I intend to rescale the time of the smoothed trajectory where needed. But it remains to be seen, whether this is really necessary. This step would be the easiest to implement.

[maxpla3]

By the way, are you aware of this? https://doi.org/10.1016/j.robot.2021.103744
They make a direct comparions to TOPP-RA. Based on the calculation times in Table 3 I assume they use TOPP-RA as a basis for their algorithm. They achieve non-time-optimal jerk-limited trajectories by linearizing the problem. The computation time increases roughly by a factor of 2 compared to TOPP-RA, which I find very reasonable.

[hungpham2511]

Hi I must have read that paper when it first came out, but I don't recall trying to re-implement it.