Invalid Trajectory: start point deviates from current robot state

[lkj1114889770]

I use descrtes in moveit. And when i execute a cartesian motion planning follow the descrates tutorial http://wiki.ros.org/descartes/Tutorials/Getting%20Started%20with%20Descartes , The errors comes in the terminal of running _ur3_moveit_config demo.launch_ , the error is:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'elbow_joint': expected: -1.43245, current: 1.6088 My ROS version is kinetic., so the ropstopic has no topic named /move_group/trajectory_execution/allowed_start_tolerance, which is a way to solve theproblem described in http://moveit.ros.org/moveit!/ros/2017/01/03/firstIndigoRelease.html for Indigo. So how to sove the problem in kinetic.

[Jmeyer1292]

@lkj1114889770 I appreciate the report. I am in the process of overhauling the tutorials associated with Descartes, which need a lot of work.

The long and short of it is: Descartes plans only for the trajectory points that you pass into it. What this means is that for a cartesian path like the one in the tutorial, the first position of the trajectory may be a long way away from wherever the robot is now.

Which drivers error out depends on the specific implementation. The way to handle this is to "connect" the start of the descartes-generated trajectory with the current position of the robot. You can do this manually by inserting a trajectory_msgs::JointTrajectoryPoint with the positions variables set to the robot's current position into the trajectory before attempting execution. You might also add a descartes JointTrajectoryPoint to the start of the path.

See this example from the industrial training repo: https://github.com/ros-industrial/industrial_training/blob/kinetic/exercises/4.1/src/myworkcell_core/src/descartes_node.cpp#L78

Feel free to respond with more questions. Again, I'm in the (slow) process of getting this stuff updated.

[lkj1114889770]

@Jmeyer1292 Thanks, It works.
According to what you said , acquire the current robot state by getting the messeage of sensor_msgs::JointState.

std::vector<double> getCurrentJointState(const std::string& topic) { sensor_msgs::JointStateConstPtr state = ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(0.0)); if (!state) throw std::runtime_error("Joint state message capture failed"); return state->position; }

By insering the codes below,and then inserts the points to the sequences of descartes points as the first point.

std::vector<double> start_joints = getCurrentJointState("joint_states"); descartes_core::TrajectoryPtPtr pt (new descartes_trajectory::JointTrajectoryPt(start_joints)); points.push_back(pt);

[yijiangh]

@lkj1114889770 Thank you for bringing this up and offering your feedback on the solution. It's really helpful!

Would you mind closing the issue if you're good on it? Just want to keep the issue list clean and focused. Thanks in advance.