EEOR - Invalid Trajectory: start point deviates from current robot state more than 0.01 相关解决办法

“Invalid Trajectory: start point deviates from current robot state more than …“ MoveIt 0.7.3+ ensures that the start state of the robot is near the current state before executing a trajectory. The check might be violated if the noise in your JointStates exceeds the default threshold value of 0.01 per joint even if your robot did not move between planning and execution. In this case you can increase the threshold value in the ros parameter set /move_group/trajectory_execution/allowed_start_tolerance - or disable the check by setting it to 0.0

http://moveit.ros.org/moveit!/ros/2017/01/03/firstIndigoRelease.html

> [ERROR] [1511284205.660285187]: 
> Invalid Trajectory: start point deviates from current robot state more than 
> 0.01 
> joint 'arm_elbow_joint': expected: 0, current: 1.84494 
 

This means the trajectory you try to execute starts far away from the current 
state of the robot. Likely your trajectory starts at the all-zero configuration 
which means whichever node plans the trajectory doesn't know, or less likely 
ignores, the current joint configuration. The most common source of error to 
trigger this would be that the `move_group` node does not subscribe to the 

correct JointState topic. 

*******************************************************

Hi all,

So this new feature has been introduced, however it is causing me all ends of problems when running the ARIAC Gazebo competition simulator.

Even though it seems we can disable or alter the threshold through setting the ROSPARAM:

rosparam set /move_group/trajectory_execution/allowed_start_tolerance 0.0
or in rospy
# Set trajectory execution ros paramters
rospy.set_param('/move_group/trajectory_execution/allowed_start_tolerance', 0.0)

It seems to have no discernible effect and I still get this error, even when I check ROSPARAM again and it is definitely set to 0.0.
#################################该方法并不能解决问题

 

Where did you find these lines? 
You should set the parameter in the launch files instead. 
The parameter is managed by dynamic_reconfigure. As far as I understood 
from its implementation, this only fetches the parameters from the parameter 
server *initially*. After that the values can/should be configured by 
ROS service calls. So if you really want to change the parameter at runtime, 
you have to call the service, e.g. by 

$ rosservice call /move_group/trajectory_execution/set_parameters "config: 
  doubles: 
    - {name: 'allowed_start_tolerance', value: 0.0}" 

##################################这种方法和上一个作用相同，也不能解决问题
 

I am using Moveit with ROS kinetic for planning the motions for a UR5 arm. I am using the Track_IK kinematic solver. The problem is , I am speicifying simple goal poses for the gripper in cartesian space, but I am getting the error message in the terminal of moveit_planning execution.launch.

[ERROR] [1511284205.660285187]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'arm_elbow_joint': expected: 0, current: 1.84494

I looked into the issue mentioned here. But I'm using trac_Ik instead of Descartes. So I am not sure how to implement a proper solution.

From that thread, it seems like the general solution is independent of whatever IK solver you're using, Trak-IK or Descartes. I think the most relevant part of that thread is this:

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 also mentioned that:

I also referenced this, but then changing the

/move_group/trajectory_execution/allowed_start_tolerance to 0.0 is giving me another error.
[ WARN] [1511285456.138267775]: Controller failed with error code INVALID_GOAL
[ WARN] [1511285456.138359326]: Controller handle reports status FAILED
[ INFO] [1511285456.138473683]: Execution completed: FAILED
[ INFO] [1511285456.168054862]: Execution request received for ExecuteTrajectory action.

in the terminal where I launch the UR_driver to interface with the real robot I have the error

[ERROR] [1511285455.959112929]: Goal start doesn't match current pose
[ INFO] [1511285455.995846536]: on_goal
[ERROR] [1511285455.996445868]: Goal start doesn't match current pose
[ INFO] [1511285456.036065982]: on_goal

This is because the controller on the UR has a similar protection mechanism that you disabled in MoveIt: ，. (it will refuse to execute the trajectory if the start state isn't the current state of the robot，（See this for some more info). I would suggest re-enabling the check in MoveIt and adding your current state to the beginning of the path you are trying to execute.

#################################该方法是可行的，在 this 有相关解答，以下是部分摘录。

       The robot is always at the same position as the driver thinks it is. Or almost, the robot send its position to the driver at 125 Hz, so the drivers perception of where the robot is may be up to 8 ms delayed, depending on when you ask the driver for that information. When the driver recieves this information from the robot, it publishes that information to /joint_states

      But this is not so relevant, as the driver checks that the starting position you have specified is identical to the position that the driver was told by the controller that the robot is at and thus, that the starting pose is identical to the information last published to /joint_states

     From your description I am uncertain on what you are doing exactly. It sounds like you want to continously stream new target positions to the robot, since you're saying that a goal is sent to the robot with a constant rate. If this is the case, I would strongly recommend that you calcualte the jacobian and use the joint_speed interface. This will give you much smoother performance, and the possibility to send new commands at up to 125 Hz.

     It also sounds like you just get the actual values from /joint_states once when the program starts, and then reuse these values. You need to update the values each time before sending a new trajectory, otherwise the values will be wrong once the robot starts moving.

So whenever you compute a trajectory, do the following step:

1. Compute IK of your target.
2. build your trajectory
3. wait for a message on /joint_states
4. update starting position based on the new value from /joint_state
5. send your trajectory to the driver

      The error you get can also occour when it takes more than 8 ms to get from the transition between step 3 and 4, and to the trajectory is recieved by the driver.
Unfortunately there is no way to gaurentee this wont happen, which is the reason I've started the discussion in #36

 

################## Hope that helps you figure out how to tackle your problem. ################