MoveIt problem. error: Trajectory message contains waypoints that are not strictly increasing in time
Hello,
I'm trying to use MoveIt's group interface to execute a Cartesian path. When I try to execute the plan with the trajectory, I get the error: Trajectory message contains waypoints that are not strictly increasing in time. I've even tried to fix it based on some code that someone else had luck with, but it doesn't work for me. Does anyone have any suggestions? Thanks! Here is my code:
// Compute the trajectory of my waypoints
moveit_msgs::RobotTrajectory trajectory;
double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory);
// All of the JointTrajectoryPoints in the trajectory have time_from_start = 0.00, which is the reason why the error gets thrown. The below steps are my attempt to fix the error, but they do nothing for me.
// First create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(move_group.getCurrentState()->getRobotModel(), "arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*move_group.getCurrentState(), trajectory);
// Thrid create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory);
plan.trajectory_ = trajectory;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
if (success) {
move_group.execute(plan);
return true;
}else{
ROS_WARN("no success");
return false;
}
Hello, did you found a solution?
To some extent, I found a solution. The solution for me was trying my code on several test cases and finding that this code actually works perfectly on some of them. I think that MoveIt has trouble finding a cartesian path most of the time.
I'm a little surprised that in some cases MoveIt doesn't have a more loud way to say that it has failed to find a cartesian path than just quietly returning a path with incorrect times.
Update: see below for possible answer.