Ask Your Question

MoveIt motion planning - How should I splice several planned motion trajectories?

asked 2019-08-13 10:42:13 -0600

clownML1234 gravatar image

updated 2020-03-02 05:30:07 -0600

fvd gravatar image

Hello guys,

When I use MoveIt to do motion planning, I want to set several middle points to make the robot arm move through all points and move to the goal.

I use OMPL planner to make the robot arm planning point to point. But obviously robot arm will stop at each point.It is not a complete and smooth trajectory.

I learned that the AddTimeParameterization can add the velocity and acceleration to each pose the planner planed. I want to splice these trajectories (only joint value) and use AddTimeParameterization to give each pose velocity and acceleration. But I don't know how to use the AddTimeParameterization in my code.

So when I get several trajectories, how can I splice them using AddTimeParameterization?

edit retag flag offensive close merge delete


Don't you mean "merging" instead of splicing? The former combines trajectories, the latter cuts them up.

gvdhoorn gravatar image gvdhoorn  ( 2019-08-15 04:01:59 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-03-02 05:52:03 -0600

fvd gravatar image

If you simply connect multiple independently planned trajectories and retime them, you are not guaranteed a smooth trajectory. In fact, the motion will likely still be jerky, unless you blend the trajectories. Blending is currently not implemented in MoveIt, but there is an open PR that includes some blending functionality internally. You may be able to use that and the existing retiming functions to blend trajectories together, and then use the IK/PlanningScene to check if they are safe. In fact, if you do, this would be a welcome PR.

To answer your question in two parts:

1) "How do I connect trajectories?"

You can connect trajectories by extracting them from the plan you made (plan.trajectory_) and concatenating them. A RobotTrajectory is simply a std::vector of JointTrajectoryPoints in C++. You can google how to connect vectors.

2) "How do I use time parameterization?":

The AddTimeParameterization you mention is a PlanningAdapter. You can see what it does here, in the source code. You will notice that it calls computeTimeStamps, the retiming function for the IPTP algorithm. There are a few more you can choose, and which are listed in this tutorial. The interface should be roughly the same for all of them: hand in a trajectory, receive a timed trajectory going through the same points.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2019-08-13 10:42:13 -0600

Seen: 258 times

Last updated: Mar 02 '20