Moveit: Improve time parameterization

[mikeferguson]

Using this to collect numerous tickets that I am closing on other repos -- this has been a common request as there are currently numerous ways to get glitches, and the current system does not respect acceleration limits

[davetcoleman]

+1 this is a much needed enhancement in moveit. @adolfo-rt wrote up a great synopsis at https://github.com/ros-planning/moveit_core/issues/167

[rbbg]

I agree that this would be a welcome improvement and would like to help out.

I was wondering about the performance of the TOPP library (If I read the paper right it states 14ms for a 7dof 1000waypoint trajectory) so I forked dave's moveit_topp repo (rbbg/moveit_topp) and created a planning_request_adapter so it can be easily used from within MoveIt!. Unfortunately it is not quite working and complains about some CLC discontinuity. @davetcoleman you probably have a little better understanding of TOPP, would you mind taking a look?

It won't be a one-size-fits-all solution to the time parameterization problem, but perhaps it can serve as a viable alternative to the current approach in some scenarios.

[rbbg]

Fixed the problem I mentioned earlier: the acceleration limits defaulted to 0.0 when they were not set in the kinematic config. I haven't done any extensive testing but TOPP usually takes <200ms on a 7dof manipulator, but I've seen it take as long as 600ms. The trajectories look pretty good but to get a better idea it will be nice to take a closer look at the accelerations that both iptp and topp will generate for an identical path. I'll probably be able to test that somewhere in the next few days.

[v4hn]

Good to hear you got it to work!
It would be nice if you could keep us updated on your results :)

[rbbg]

I have made a small test script to store and compare identical trajectories using the different parameterization methods. The next two figures demonstrate the difference between IPTP and TOPP, I think you can guess which is which :) The acceleration limit is unspecified and thus set to 1.0, so they seem to get violated by both methods, but IPTP is worse.

Note that the duration of the TOPP path is also a little shorter, usually shaves off a couple of % if I had to guess, it's not a huge difference.

This is just one example; sometimes the difference is smaller, sometimes bigger, but from what I've seen so far, the TOPP path consistently has fewer acceleration spikes. Total parameterization time seems to average around about 200ms for most of the things I throw at it, but perhaps that can be improved as well.

[gavanderhoorn]

A bit of a 'me too +1' post, but I must say that subjectively, I like the TOPP vel & accel profiles a lot better than those out of IPTP. It also looks like the position curve is 'much' smoother, which would seem to suggest a nicer motion (although we only see a single joint here).

[avoelz]

As a side note, the algorithm described in "Time-Optimal Trajectory Generation for Path
Following with Bounded Acceleration and Velocity" by Tobias Kunz and Mike Stilman is available as C++ code at https://github.com/tobiaskunz/trajectories. It has no dependencies except Eigen and is thus very easy to integrate. I used it as alternative to IPTP and was very content with computation time and resulting trajectories.

[v4hn]

On Tue, Sep 13, 2016 at 03:30:54AM -0700, avoelz wrote:

It has no dependencies except Eigen and is thus very easy to integrate.
I used it as alternative to IPTP and was very content with computation time and resulting trajectories.

This looks pretty good to. Do you have code around for the integration of this in MoveIt?
As this is BSD-2 clause license, we could definitely integrate this with MoveIt.
Could we integrate these algorithms as PlanningRequestAdapters?

[davetcoleman]

+1 to using Tobias' work, I've talked to him about it and it sounds very promising

[mikeferguson]

We actually have a moveit-wrapped version of tobias work that we use in-house as a planning request adapter (with tests even). Let me see if we can release that.

[mikeferguson]

Ok -- I've got a thumbs up on releasing -- what folder would this go into? I'm thinking "moveit_ros/planning/parabolic_trajectory_adapter", any issues with that? (I know the other time parameterization is in planning_request_adapters, but there is actually quite a bit of code/tests for this new adapter compared to the old one, not sure we actually would want to mix it into that...)

[gavanderhoorn]

Do you have any plots available? I'd be interested to see the difference with IPTP.

[davetcoleman]

Actually, the best place for it would be next to our current time parameterization implementation in moveit_core/trajectory_processing/ but I presume your implementations have more ROS dependencies than moveit_core allows?

Great news about the open sourcing of your work, btw!!

[mikeferguson]

@gavanderhoorn -- I don't have any plots on hand, but I'll try to capture some when I get a chance.
@davetcoleman -- you might be right, I think I could actually separate it that way (let me look and see)

[BrettHemes]

I found a bug in the current time parameterization that could be what you guys are seeing in all of the referenced issues....

Unless I am mistaken, there is an index-off-by-one error in IterativeParabolicTimeParameterization::updateTrajectory at line 206 here.
rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i-1]);
should be
rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i]);

I found this while analyzing some of my trajectories after by comparing pre and post "parameterized" trajectory time stamps. Even after commenting out the apply constraints lines in computeTimeStamps and just performing the following:

  getTimeDiff(trajectory, time_diff);
  updateTrajectory(trajectory, time_diff);

the issue remained and then tracked it down to the line mentioned above in updateTrajectory.

The shift in times results in jerky trajectories even after the parameterization (which actually works fine)

[davetcoleman]

:joy: if your finding is correct, that's amazing. can you propose it as a PR?

[BrettHemes]

Sure, I am still investigating but will keep you updated and submit a PR
if I get it all figured out.

[BrettHemes]

Ahh, false promises. I was working my fork of the code and found a bug in an old modification of mine. Sorry for jumping the gun there... I got too excited.

[davetcoleman]

@mikeferguson any progress on releasing Fetch's parameterization plugin? this is something i would personally utilize greatly.

[davetcoleman]

I just wrote up a short tutorial on this feature: https://github.com/ros-planning/moveit_tutorials/pull/50/files

[gavanderhoorn]

Same here. Super :smile: :bellhop_bell: ?

[rbbg]

So I did some more work to figure out what the problem with IPTP is as I kind of like the approach. It may not give you an optimal trajectory, but it is really fast (order of 1ms) and fairly reliable in execution time. Consider the following image of an IPTP trajectory:

To clarify: the green dots are the target positions, velocties and accelerations at the waypoints, and the blue lines are the interpolation that is performed by the controller.

What's going on here is that the IPTP first sets the time stamp in a iterative manner with the velocity and acceleration constraints. After the timestamps have been set, a heuristic approach is used to set the velocity and acceleration goals on the waypoints. Obviously the quintic interpolation that the controller uses isn't playing nice with the velocity and acceleration waypoints, causing these spikes.

Instead of using the heuristic to set the velocities, I fit a cubic spline on the waypoint positions and time axis, and evaluate the first and second derivatives on the waypoints to set the goal states. Following figures are parameterizations on the same trajectory

IPTP

IPTP + Spline

This works, but the acceleration limits ( -pi : +pi) are not satisfied in either case. However, as the controller will now never interpolate to a higher acceleration than specified in your waypoints, you do remove the strange behavior where the accelerations spiked in between waypoints and maximum values on the accelerations are not as high as with the standard IPTP.

Perhaps with some small improvements, IPTP can be used as a non-optimal-but-fast parameterization, and TOPP/TOTG (Time-optimal Trajectory Generation?) can be used for optimal-but-slower parameterization.

[gavanderhoorn]

@rbbg: just to compare: could you add a plot showing TOTG 'performance' for a similar trajectory?

[rbbg]

Yeah, this is a trajectory from a TOTG parameterized trajectory (wrote a very simple wrapper for testing):

Note that the algorithm doesn't provide acceleration information, but it's not strictly needed. The small spikes that do violate the trajectory are related to the sample density of the trajectory, and can most likely be fixed.

[v4hn]

@rbbg Your result looks quite good. Would you mind to create a pull-request for your spline-derivative approach?
Even if we decide not to merge your code as-is, it would help to review a concrete approach to improve the acceleration values for our IPTP plugin.

• Which controller do you refer to in your analysis above?
• How did you generate the graphs? They look pretty neat :-)

[rbbg]

I will look into it. Currently I am using this GPL licensed spline fitting library, so I suppose I would have to change that? Any suggestions for a (cubic) spline fitting library that wouldn't be too hard to use from within MoveIt!?

As for the controller: I have been using the joint_trajectory_controller from ros_controllers. This broadcasts a desired state which is essentially the output of the quintic interpolation. I did have to increase the publish rate of the joint_state_controller/JointStateController in order to make sense of the interpolation. To make the figures, I made a rosbag of the execution and wrote a python script to parse and plot the trajectory waypoints overlayed over the desired states.

[davetcoleman]

So glad you're working on this! As @v4hn suggested a pull request for this would be great. We could have this along side our current time parameterization algorithm and if, after being tested for a while with positive review, we could switch our default one to yours. My new tutorial could be updated to explain how to make the switch.

I also used a GPL spline fitting library when I attempted this problem. You're right that this does not work well with our BSDv3 licensing [disclaimer: I'm not a lawyer].

[v4hn]

@mikeferguson

Ok -- I've got a thumbs up on releasing -- what folder would this go into?

Any chance we might get a patch for this as a Christmas present? ✨
If it's not ready to merge, that does not matter too much. Someone else can clean it up if you don't have the time.
Personally, if you would send me a tarball/git-checkout of a package, I could spare some time to make a request from that..

[johnnieleung]

@rbbg Hi Ruben, first of all, thank you for writing this MoveIt! plugin of TOPP (rbbg/moveit_topp)! However, it seems there is a very small problem in your code here:

ROS_WARN_STREAM_NAMED(name_, "No acceleration limit specfied, using 1.0");
acc_limits.push_back(1.0);

I believe there should be braces grouping these two lines (line 67 and 68), otherwise it will cause error when the user already specified acceleration in joint_limits.yaml.

And another question is that, when using this trajectory processor, I occasionally encoutered the error "CLC failed", which I had no idea why. It happens quite seldom but it does happen and without digging too deep in the algorithm I had no clue what caused it so it is a little bit difficult to reproduce the problem. Do you have any idea about the cause? Thank you very much!

[rbbg]

Hi @johnnieleung, I would like to stress that that I forked moveit_topp from @davetcoleman and turning it into a plugin was just a minor effort.

Thanks for the heads up on the typo, I only tested with configurations that didn't have the acceleration limits set so never noticed it but it should be fixed now.

With regards to the CLC error, that stems from TOPP itself and there are several related issues on the issue tracker of the original repo, for instance TOPP#26.

[davetcoleman]

I would strongly encourage any help in moving this topp plugin into the main moveit repo - if you open a tested pull request I would review and do additional testing.

[rbbg]

I'll see what I can do. I don't think it is quite ready right now. It uses another GPL spline fitting lib, as you know, so that would have to change.

[davetcoleman]

We could probably use the same spline library we are using in the other parameterization pull request in the works - the author agreed to a BSD license:

https://github.com/ros-planning/moveit/pull/382#issuecomment-267446099

[aiolosxuk]

Hi @rbbg, I tried t use your forked moveit_topp to plot moveit trajectory. But I found the velocity curve was always trapezoidal. The first figure is IPTP (Using quintic polynomial) and the second one is TOPP (plot TOPP::Trajectory directly with 0.004 dt). They are plotted from identical trajectories.

Is it because we set the same time stamp interval for all the waypoints on MoveItTopp::convertMoveItTrajToPP() function?

I noticed @davetcoleman moveit_topp demo uses the matlab to pre-generate piecewise polynomial. But I think it cannot be used to convert the moveit trajectory.

[rbbg]

That is correct. The profile your seeing is exactly what a velocity and acceleration bounded time-optimal trajectory _should_ look like. I think moveit_topp does fit a piecewise polynomial over the moveit trajectory before the parameterization, but I would have to check.

[renarded]

I’ve had a problem which I think is related to this issue. My problem looks like the same as https://github.com/ros-planning/moveit_core/issues/167

The weird thing is : on Indigo everything is working correctly, I have nice velocity for every trajectory. The problem only occurs on Kinetic, for something like 50% of trajectories. I’m running moveit with default kdl planner.

Here’s the plot of the velocity (from Moveit! trajectory) on Indigo :

Velocity is increasing, then staying constant, then decreasing. Everything is normal

And here’s the plot in Kinetic, for the same move, with the same URDF :

The velocity is just going crazy. Sometimes it makes the robot almost stop multiple times for one trajectory.

Is it possible to say to Moveit : “I want the velocity to increase, then arrives to max velocity, then decrease” ?

What could possibly change between indigo and kinetic ?

[gavanderhoorn]

I can't get github to work with me, but a local diff between the current indigo-devel and kinetic-devel versions of iterative_time_parameterization.cpp shows no differences, so it would seem that what you are reporting is possibly the result of something else.

[v4hn]

@renarded do you take the velocities you plot there directly from the generated trajectory or is this a recording of the trajectory actually executed (i.e. filtered through the hardware controller)?
Does the velocity of all planned joints fall off at the same time in between? Your plot shows only one of the controlled joints..

[renarded]

@v4hn The velocity is directly coming from Moveit! trajectory (I use a cubic interpolator to plot them, which doesn't modify the trajectory behavior).
I've created an issue here to explain my problem. I don't really know if it is directly link to the time parameterization issue, or another thing inside Moveit! planning.

[davetcoleman]

@renarded nothing has changed for the trajectory parameterization functionality AFAIK. Are you sure the input trajectory is the same discretization level? If you are planning the trajectory through OMPL planners, they will likely be different. There is also a change in Kinetic that affects the discretization of generated OMPL trajectories: https://github.com/ros-planning/moveit/pull/337

[gavanderhoorn]

@davetcoleman: #416 explains the issue @renarded is running into. It looks like it's actually a problem / change in OMPL (bitbucket.org/ompl/ompl#309).

[BrettHemes]

@davetcoleman you mentioned talking with Tobias about TOTG and have worked with TOPP. Why would one prefer TOTG over TOPP?

Edit: Sorry, that might have been an unfair question... they are both based on the same underlying approach and, after reading their respective publications, I was unclear (from a theoretical standpoint) on when TOTG would win. After playing with each of the implementations, however, I can see how each implementation has its own set of pros and cons.

[davetcoleman]

Yes that's a hard question to answer from memory - I have not read those papers in many months (maybe a year)...

[mmoerdijk]

This maybe a dumb question, but doesn't every cnc have the same problem of moving multiple axis simultaneously?

Why can't we use solutions from the cnc world?

[davetcoleman]

I think there is potential and I attempted to read into their literature once. However the CNC world has a much lower number of DOF so that might affect some of the assumptions they have made.

[v4hn]

@mmoerdijk in other words: feel free to look around and contribute!
Just because there are long threads of discussion, that doesn't mean it's too complex if you know where to look...

[mikeferguson]

I realize we've already got TOPP in Kinetic,but I'm working on TOTG right now, so I'm going to assign this to myself, since I think I've only mentioned I'm working on this to @davetcoleman.

The current TOTG branch I have is targeting Indigo, but a port to Kinetic will be easy. That branch is here: https://github.com/mikeferguson/moveit/tree/totg.

I've tested quite a bit today, but I have found an issue in that the pick and place pipeline lacks the joint velocity/acceleration limits -- so the plans that come out are stupidly slow. Once I've tracked down what is going on there, I just need to clang the code and update the logging calls (since I think they are now out of date). Hoping to figure out the pick/place problem tomorrow.

[lakshmip001]

@aiolosxuk I implemented moveit_topp on my moveit trajectory as I need trapezoidal tool velocity.I dont know how I can get trapezoidal tool velocity when I constrain joint acceleration and joint velocity using moveit_topp. Is the trapezoidal velocity resulting in tool velocity of manipulator also?

[sabaghian]

In Iterative cubic spline time parameterization (iterative_spline_parameterization.cpp ), in function global_adjustment_factor(), the velocities are only checked at boundaries. Since the velocity is a second order polynomial, it can have another maximum value which can easily be checked too.

[itfanr]

@rbbg Thank you for your implemention of TOPP. I will try it on my robot.

[mikeferguson]

I think we can actually close this now that we have several methods ported in (the most recent one in https://github.com/ros-planning/moveit/pull/1365)

[itfanr]

@mikeferguson I will read the issue #1365