Path time parameterization yields non-smooth time evolution

[adolfo-rt]

There's been some discussion on the list on the subject of motion plans being less smooth than expected (c.f. Planning directed path plans thread). So far the discussion has revolved around path simplification, i.e., paths not being shorter or more direct, and I believe there is definitely room for improvement in this front.

This issue aims at raising another aspect of non-smooth motion plans, those concerning the plan's time parameterization. I've observed that even in the cases where the path quality is decent, its time evolution is not, resulting in high velocity/acceleration variations.

I'd like to look into this, but would appreciate some pointers to get a warm start. My intention is to start with the trajectory_processing part of moveit_core. Plan B is to determine whether path simplification is creating small ripples in the path that when differentiated cause the observed velocity/acceleration variations.

---

The below example illustrates the issue rather well. The robot is performing self-collision avoidance, and the trajectory_msgs::JointTrajetory part of the motion plan generated by MoveIt! is here.

Notice that although the trace of the arm wrist looks fine (3D rendering, right), an inspection of the desired position, velocity and acceleration (plot, left) tells a different story. The desired velocity has high variation and many direction reversals. Acceleration variations are also very high.

Note: The desired acceleration plot is exaggerated, as rqt_plot's natural spline interpolator is overshooting between the data samples. To have a more accurate idea of how acceleration is changing, consider the particular example of the transition between the fourth and fifth trajectory points for the first arm joint:

positions: from 0.891 to 0.889
velocities: from -0.049 to -0.059
accelerations: from 0.688 to -0.578
duration: 0.132s

I've run the test enabling (set to 1rad/s^2) and disabling acceleration limits with similar results. If relevant, the complete moveit config can be found here.

[davetcoleman]

Hi Adolfo, sorry for the late reply.

This is a very much needed improvement for MoveIt! from my experience as well. The current implementation of trajectory processing, as I have heard from some of the other developers as well, is pretty bad.

There was an attempt to integrate Reflexxes library here (in the comments), but I do not believe there is any other progress. If you recall, myself and @jbohren also attempted last summer to create a Reflexxes trajectory controller, which is not exactly the same as time parameterization, here.

I can't offer much other insight into this issue, but would be interested in helping where possible.

Thanks!

[skohlbr]

We recently also noticed that there are pretty big variations (very visible when looking at accelerations computed by the ros_control JointTrajectoryController). Does anybody know why the development of the Reflexxes-based time parametrization stopped? I´d especially like to know if there are some major obstacles, or if it could be made to work with reasonable effort.

[davetcoleman]

I believe it would work, it just was never finished. It would be a big improvement.

[adolfo-rt]

There are two sides of the coin here.

1. MoveIt's time parameterization should yield cleaner trajectories. If it did so, the underlying controller should have no problem realizing the sent command.
2. Implement a variant of joint_trajectory_controller where the trajectory segment representation and interpolation leverages Reflexxes' type II library. If you want to discuss this approach in more detail, we can do it in a separate issue at the ros_controllers repo.

[skohlbr]

Ok, I looked into Reflexxes a little more and it appears that the main difficulty is that, while it provides the capability to generate instantaneous controls for a trajectory segment given current and target state, it does not provide the capability to perform time parametrization. In the commented out code in add_time_parametrization.cpp, the target velocity for a trajectory segment is just set to zero so far, which is essentially dodging the problem of time parametrization (and probably the reason why the code is commented out :) ).

[adolfo-rt]

@skohlbr, I'm not sure I understand your comment. Why does time parametrization depend on the boundary conditions, like waypoint velocity?. My understanding is that Reflexxes would compute the shortest-time trajectory to reach the desired boundary conditions, without violating specified limits. For multi-dof trajectories, the time synchronization capability would be used, to make sure all joints reach the goal at the same time.

On a related note, I presume Reflexxes time synchronization is doing a time warping that slows down all joints to meet the duration of the most restrictive one (the one with longest duration). I mention this because a similar warping strategy could be used for the much-requested feature of specifying a (lower bound for the) trajectory execution time.

[skohlbr]

@adolfo-rt Ok, the "it does not provide the capability to perform time parametrization" comment was indeed misleading/wrong. Reflexxes allows computing a shortest time trajectory to reach desired boundary conditions, but those have to already be defined. The main problem is the velocity in the target configuration. This is just set to zero in the currently commented out code in add_time_parametrization. So for every segment the joint would come to a standstill, then accelerate/decelerate again for the next segment etc. It is unclear to me how and if this problem of coming up with velocities (and possibly accelerations) for the trajectory waypoints could be solved using Reflexxes.

[adolfo-rt]

Ah, yes, I get your point. Reflexxes is meant to be used to reach a predefined goal, but it does not lend itself to optimize the timing of a whole trajectory. Is this what you mean?. @isucan, any additional thoughts that might be relevant here?.

An alternative would then be to implement option 2. of this comment. If Reflexxes is used in the low-level controller, then it will be always reaching the next waypoint (which is known) from the current state. @jbohren did something along these lines here.

The above implies that the current MoveIt! implementation would still need to be improved, but I understand that there is room for it. Considering that the algorithm is iterative, I would first try to simply increase the number of iterations and see what comes out.

[sachinchitta]

We had tried to figure out how Reflexxes would integrate for this but, as @skohlbr mentioned, it does not compute velocities and accelerations for intermediate waypoints, so its not the best fit for this case. Option 2 that you mention will only process each segment of the trajectory generated from MoveIt! - so if the initial trajectory is already not looking good, it won't really help since you are not really changing anything about the global structure of the trajectory.

[dcconner]

I took a stab at recalculating the velocities and accelerations to make them consistent with piecewise cubic splines (as test before looking to quintics used by JointTrajectoryControllers). First I run the current AddTimeParameterization MoveIt plugin to get the piecewise linear segments within velocity/acceleration limits, then do a smoothing step by recalculating the interior velocity and acceleration values. This results in a smooth curve but one with too much wiggle. For example, starting with a simple trapezoidal velocity profile will end up with an oscillation instead of constant velocity term.

Doing this correctly will require interplay between the trajectory smoothing, time parameterization, and spline fitting. At this point, I'm leaning toward switching to a piecewise linear trajectory following and scrapping the quintic splines until we have time to dig into the smoothing/sampliing issues deeper.

[dcconner]

1) Back to @adolfo-rt original comment. Since MoveIt! is not using quintic splines under the hood, the quintic coefficients calculated by ros_controllers are not consistent with the piecewise linear segments assumed by the AddTimeParameterization. I tried recalculating the interior velocities and accelerations with a mod to the AddTimeParameterization plugin; this gives "smooth" but still undesirable effects as mentioned above. We may revisit this by digging into the trajectory sampling code; but that will have to wait.

2) In the meantime, I've implemented a linear_spline_segment.h to work with ros_controllers/JointTrajectoryController in our fork. It is based quintic_spline_segment.h, but ignores specified velocity and acceleration whether present or not. It calculates velocity based on constant ramp between position waypoints, and sets acceleration to zero. We'll be testing tomorrow to see if this plays nicer with MoveIt! trajectories and our controls. I expect more overshoot than if we had valid quintic spline segments, but will simplify our testing. If it seems beneificial, I'll clean it up and issue a pull request in ros_controllers.

[davetcoleman]

+1 Thanks for looking into this!

[adolfo-rt]

@sachinchitta

Option 2 that you mention will only process each segment of the trajectory generated from MoveIt! - so if the initial trajectory is already not looking good, it won't really help since you are not really changing anything about the global structure of the trajectory.

Option 2 is orthogonal to MoveIt!. It would be a mechanism to enforce joint limits at the joint_trajectory_controller level. So yes, it will not help making MoveIt!'s output better, but it would do a better job at filtering out extreme velocities or accelerations (or jerks, depending on the Reflexxes version).

@dcconner The IterativeParabolicTimeParameterization defaults to 100 max iterations. This default does not seem to be overridden anywhere, it's not even configurable from the outside. Before spending a lot of time trying entirely different alternatives, how about simply cranking up that number and see what happens?.

[davetcoleman]

Considering that the algorithm is iterative, I would first try to simply increase the number of iterations and see what comes out.

@adolfo-rt I cranked up that number as you suggested. I've looked at the data a lot of ways, but this plot sums up my takeaways - increasing the number of iterations actually increases the accelerations spikes that I'm having issues with. The largest wave in this picture is from 460 iterations.

I'm having a jerk/jitter problem with my controller/robot. Not sure if it lies within the IterativeParabolicTimeParameterization as discussed here, or perhaps related to the bug in the JointTrajectoryController that occurs when I preempt new trajectories at a high rate (~100hz).

I just thought I'd share this result for others who might be curious the result of this. Also, has anyone made progress on this long-standing issue?

[alainsanguinetti]

Hello, just to let you know that I am working on it for my current company. Here is a bit of literature on the subject : as you will see, it's an open question but there is trend toward S-curves and analyzing a path by segments. https://en.wikipedia.org/wiki/Jerk_(physics) http://homepages.laas.fr/daniel/iros06WS_Herrera.pdf http://www.motion-designs.com/images/DTrends_Aug_2010.pdf http://cdn.intechopen.com/pdfs-wm/4267.pdf http://www.arscontrol.org/uploads/events/SeminarioDismi130206.pdf

I have also made some successful trials with non-linear optimization solvers ( with few 3 points and one joints ), I am attaching the file that can be run on http://apmonitor.com/online/view_pass.php 2seg_vel.txt.

As far as I am concerned I have the feeling that this problem of a time optimal multi-points multi-axis trajectory could be solved by linear algebra but this is probably a thesis project. In the mean-time we probably can do better than this implementation, what is beginning to work for me is to divide the path into segments where all the axis each follow the same direction. Then for each segment find the longest axis and then create a trapezoidal motion using this time. After that, it would be quite easy to resample the trajectory at any rate.

I am not publishing the code and I actually created my own planning request adapters, however, when I come to a satisfactory stage in my company, I think I will be able to publish the code for this.

[davetcoleman]

That's great to hear!

I worked on an experimental solution to this last January using Time Optimal Path Parameterization - it seemed like a good solution and I had a working demo, but the code I built my MoveIt! interface on top of was too slow for anything close to realtime results (took several seconds). Here's my progress: https://github.com/davetcoleman/moveit_topp

Original implementation: https://github.com/quangounet/TOPP

[mikeferguson]

https://github.com/ros-planning/moveit/issues/160