Invalid Trajectory: start point deviates

[v4hn]

This reports problems with #63 in practice.

I just tested current indigo-devel with our PR2 and I often get these errors for the execution of trajectories (only one each time):

[ERROR] [1476283190.409951701]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'l_forearm_roll_joint': expected: 2.62748, current: -3.6557

[ERROR] [1476283258.133136672]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'l_forearm_roll_joint': expected: 0.469322, current: -5.81421

This arises from un-normalized joint values of the unlimited joints on the PR2's arm, that are not taken into account. This is a systematic problem, and imho not just specific to the PR2.

Also I think it's worth to note that it is still necessary to update the start state manually when planning in RViz, because the update happens too early after the execution (the rviz plugin does not have a current state of the robot yet). Without updating, I keep getting errors such as

[ERROR] [1476283118.443709588]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'l_elbow_flex_joint': expected: -1.17855, current: -1.18984

[ERROR] [1476287101.452005358]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'l_upper_arm_roll_joint': expected: 3.67128, current: 3.69101

If we should decide to release into indigo without #232 I consider it reasonable to add a sleep to the plugin until this is fixed properly by #232.

As the first type of normalization error breaks working setups (at least the PR2), I consider this a blocker for the indigo as well as the kinetic release.

I'm really sorry to keep on adding release blockers. :(

[davetcoleman]

This arises from un-normalized joint values of the unlimited joints on the PR2's arm, that are not taken into account.

So we need to modify the start point deviation check to account for un-normalized joint values, correct?

[v4hn]

On Wed, Oct 12, 2016 at 01:32:59PM -0700, Dave Coleman wrote:

This arises from un-normalized joint values of the unlimited joints on the PR2's arm, that are not taken into account.

So we need to modify the start point deviation check to account for un-normalized joint values, correct?

I suppose... But it's not totally trivial because I suppose we should only normalize unlimited revolute joints.
I'm not sure this information is available at this point in the code.

[rhaschke]

I have no idea, where the information about unlimited or not normalized joints is available from the robot model or state.
Alternative to comparing joint values, we could compare the Cartesian pose of all robot links after applying the forward kinematics transform. This would also relax the problem of the single threshold value 0.01, that doesn't generalize well across linear and revolute joints.

[davetcoleman]

In revolute_joint_model.cpp there is a notion of continuous (unlimited) joints and a function enforcePositionBounds(). In that function a continuous joint value is normalized to be between -PI and PI. Seems to me you should call that function first on all joints before checking for deviation from start state?

[v4hn]

Alternative to comparing joint values, we could compare the Cartesian pose of all robot links after applying the forward kinematics transform. This would also relax the problem of the single threshold value 0.01, that doesn't generalize well across linear and revolute joints.

This is an intriguing idea, but I would not go done that road. The threshold value would not be directly related to the joints anymore and the value would first and foremost depend on the length of your links and your kinematic chain.

Instead I would propose to make the noise-level configurable per joint and add a method to JointModel (with defaults for each type) that returns that value or even directly decides whether the joint has the same value as an input +- noise.

In revolute_joint_model.cpp there is a notion of continuous (unlimited) joints and a function enforcePositionBounds(). In that function a continuous joint value is normalized to be between -PI and PI. Seems to me you should call that function first on all joints before checking for deviation from start state?

That sounds reasonable. I'll try to test this tomorrow in the lab.

[rhaschke]

@v4hn You should focus on #273. I will have a look into a fix here (today evening).

[v4hn]

too late, sorry.. I'll propose a request in a moment...

[rhaschke]

Fixed by #290 and #292.

[crvogt]

Hi there,
This issue is coming up for me. I've done a clean ROS install, Indigo on Ubuntu 14.04, but it's continued to persist. I followed the instructions in this link http://moveit.ros.org/moveit!/ros/2017/01/03/firstIndigoRelease.html (I've both built it from source and tried the binaries. I've checked #290 and #292, as well as checked the actual changes made to the file to ensure I have built the most recent version with the currentPose - trajectoryPose line.

The error occurs consistently when running a simple node and the pr2_moveit_config demo.launch file.

Is it that moveit-full is still not offered, and that by downloading it I still have the old code with this bug still?

Any help is much appreciated :)

[v4hn]

@crvogt Please paste the exact error message you see and explain what exactly you did to cause it. This error can arise for a number of reasons. Feel free to file a new issue for it too.

[crvogt]

@v4hn Thank you for the quick response.

I was a bit hesitant to make a new issue for it, but I suppose posting in the appropriate place makes sense :)