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;
}
↧