Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improved IPTP by fitting a cubic spline #382

Merged
merged 5 commits into from Jan 27, 2017

Conversation

Projects
None yet
4 participants
@rbbg
Copy link
Contributor

commented Dec 14, 2016

As discussed in #160. I have used a GPL licensed spline fitting library (this one) to improve the performance of the iterative parabolic time parameterization algorithm that is used by MoveIt!. Unfortunately, the acceleration limits are still not satisfied completely, but it appears to be a step up from the previous approach.

Problems:

  • The license. I am not too well versed in the what/why/how of open source licensing but I don't think GPL and BSD are compatible. Perhaps someone more knowledgeable on this front can offer some insight? If anyone is aware of a BSD licensed spline library that'd be great.

  • The acceleration boundary conditions are not 0. This causes the waves you see between the first and second waypoints in the following figure:
    splinefit

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Dec 14, 2016

I have emailed the Spline author @ttk592 asking if he would be ok changing the license to BSD.

@rbbg

This comment has been minimized.

Copy link
Contributor Author

commented Dec 14, 2016

That'd be pretty neat.

I am still pondering the issue with the first waypoint. I'd really like to figure out a way to make it work as it would mean you actually know what your maximum velocities and accelerations are, and you could use that information to perform a parameterization that is guaranteed to not violate the limits on the controller side.

@v4hn

This comment has been minimized.

Copy link
Member

commented Dec 15, 2016

@rbbg Thanks for providing the pull-request!

Do you already got permission to change the license? If you can't point to some text by the author that allows you to do that, please undo it in the pull-request again! I know it's frustrating, but software licensing is nothing to toy with in our case. We definitely do not want people not to use the project because of murky licensing practice...

I do not yet understand where your "first waypoint" issue stems from.
Would it help to add another (zero-acceleration) waypoint in front of the trajectory before fitting?

@rbbg

This comment has been minimized.

Copy link
Contributor Author

commented Dec 15, 2016

Hi @v4hn, Dave cc'd me in the e-mail he sent to the author of the library, and he agreed to the change (for MoveIt!). Dave asked me to change the license so @ttk592 can review it and comment on this PR confirming that the change is ok.

Regarding the first waypoint, I think it might be related to the controller "dropping the first waypoint because it occurs before the current time". I'll run some tests and will report back. I tried adding another waypoint or manually changing the acceleration limit but that didn't help.

@davetcoleman
Copy link
Member

left a comment

Can you provide a quick comparison of processing time between old approach and the new approach, for an average sized trajectory?

@@ -211,81 +208,26 @@ void updateTrajectory(robot_trajectory::RobotTrajectory &rob_trajectory, const s
if (num_points <= 1)
return;

// Accelerations
for (int i = 0; i < num_points; ++i)
// Spline fitting using GPL lib

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Dec 15, 2016

Member

maybe not call this "GPL lib" but something like "cubic spline library"

@@ -35,6 +35,7 @@
/* Author: Ken Anderson */

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Dec 15, 2016

Member

Given your approach is still experimental, I think we should not change Ken Anderson's itarative_time_parameterization version but rather add a secondary approach that can be optionally changed to (and documented). So I think you should copy this file and call it something different. You will also need to copy the planning request adaptor plugin and name it something different. This will ensure we don't change the behavior of currently deployed systems.

This comment has been minimized.

Copy link
@rbbg

rbbg Dec 15, 2016

Author Contributor

Yeah, I figured this to be more of an initial testing version, but if it can actually be merged I will move it to another file and give the plugin another name.

Updated spline.h license to BSDv3.
Updated spline.h license to BSDv3.

Clang-format.

Fixing license errors in spline.h

@rbbg rbbg force-pushed the rbbg:iptp_spline branch from e6833c8 to 941c225 Dec 15, 2016

@ttk592

This comment has been minimized.

Copy link

commented Dec 15, 2016

The license change to BSDv3 in this commit is fine by me.

@rbbg

This comment has been minimized.

Copy link
Contributor Author

commented Dec 16, 2016

As for a quick comparison of the performance: a 7dof manipulator planning around some obstacles(usually around 20-30 states): they both run in 1-2ms and the Spline approach is usually around 10-20% slower, so 1.7ms for Spline, 1.5ms for the original approach. In both cases, the iterative procedure to determine the time differences between waypoints takes the most time (The original heuristic setting of the velocities and accelerations takes <100us)

@rbbg

This comment has been minimized.

Copy link
Contributor Author

commented Dec 16, 2016

I fixed the problem with the first waypoint by prefixing a zero acceleration waypoint 1ns before the start waypoint, after fitting the spline.

One problem remains, with the current starting timestamps, the first waypoint often gets dropped by the controller because it occurs in the past, when this happens, the controller will again apply the quintic interpolation and you get the 'wave' as seen in the figure above. I've added a 50ms offset for now to ensure this doesn't happen, but that's not a very good solution.

Currently the trajectories are looking like the following figure (acc limit = 1.0).

figure_3

Still not satisfying the joint limits, but at least the acceleration is limited by the values that are in the waypoints.

I have named the plugin SplineParameterization and it can be added to the pipeline by default_planner_request_adapters/AddSplineParameterization.

*/

#ifndef MOVEIT_TRAJECTORY_PROCESSING_SPLINE_SMOOTHER_
#define MOVEIT_TRAJECTORY_PROCESSING_SPLINE_SMOOTHER_

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Dec 16, 2016

Member

Should be SPLINE_PARAMETERIZATION_

@rbbg

This comment has been minimized.

Copy link
Contributor Author

commented Jan 24, 2017

I have made the requested changes, anything else required?

To clarify: This only adds a parameterization method, so the existing IPTP method remains unchanged.

@davetcoleman davetcoleman merged commit 0d673c6 into ros-planning:indigo-devel Jan 27, 2017

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
@davetcoleman

This comment has been minimized.

Copy link
Member

commented Jan 27, 2017

@rbbg can you please summarize the benefits discussed in this thread and document them in the Parameterization Tutorial I started? http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/time_parameterization_tutorial.html

Thanks!

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Jan 27, 2017

Improved IPTP by fitting a cubic spline (ros-planning#382)
* Added spline library and IPTP spline fitting.

* Updated spline.h license to BSDv3.

Updated spline.h license to BSDv3.

Clang-format.

Fixing license errors in spline.h

* Restored IPTP and added Spline adapter.

* Updated license information.

* Changed ifdef name of header file.

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Jan 27, 2017

Improved IPTP by fitting a cubic spline (ros-planning#382)
* Added spline library and IPTP spline fitting.

* Updated spline.h license to BSDv3.

Updated spline.h license to BSDv3.

Clang-format.

Fixing license errors in spline.h

* Restored IPTP and added Spline adapter.

* Updated license information.

* Changed ifdef name of header file.

@kennethanderson kennethanderson referenced this pull request Feb 11, 2017

Merged

Iterative cubic spline #441

1 of 5 tasks complete

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Feb 14, 2017

Improved IPTP by fitting a cubic spline (ros-planning#382)
* Added spline library and IPTP spline fitting.

* Updated spline.h license to BSDv3.

* Updated spline.h license to BSDv3.

* Clang-format.

* Fixing license errors in spline.h

* Restored IPTP and added Spline adapter.

* Updated license information.

* Changed ifdef name of header file.

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Feb 14, 2017

Improved IPTP by fitting a cubic spline (ros-planning#382)
* Added spline library and IPTP spline fitting.

* Updated spline.h license to BSDv3.

* Updated spline.h license to BSDv3.

* Clang-format.

* Fixing license errors in spline.h

* Restored IPTP and added Spline adapter.

* Updated license information.

* Changed ifdef name of header file.

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Feb 14, 2017

hartmanndennis added a commit to iirob/moveit that referenced this pull request Oct 23, 2017

davetcoleman added a commit that referenced this pull request Jan 27, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.