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

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?

Asked by clownML1234 on 2019-08-13 10:42:13 UTC

Comments

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

Asked by gvdhoorn on 2019-08-15 04:01:59 UTC

Answers

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.

Asked by fvd on 2020-03-02 06:52:03 UTC