New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Invalid Trajectory: start point deviates #283

Closed
v4hn opened this Issue Oct 12, 2016 · 11 comments

Comments

Projects
None yet
4 participants
@v4hn
Member

v4hn commented Oct 12, 2016

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. :(

@v4hn v4hn referenced this issue Oct 12, 2016

Closed

Initial Indigo release from ros-planning/moveit repo #100

15 of 15 tasks complete

@davetcoleman davetcoleman referenced this issue Oct 12, 2016

Closed

Kinetic Release #18

19 of 19 tasks complete
@davetcoleman

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Oct 12, 2016

Member

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?

Member

davetcoleman commented Oct 12, 2016

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

This comment has been minimized.

Show comment
Hide comment
@v4hn

v4hn Oct 13, 2016

Member

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.

Member

v4hn commented Oct 13, 2016

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

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Oct 14, 2016

Contributor

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.

Contributor

rhaschke commented Oct 14, 2016

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

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Oct 14, 2016

Member

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?

Member

davetcoleman commented Oct 14, 2016

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

This comment has been minimized.

Show comment
Hide comment
@v4hn

v4hn Oct 16, 2016

Member

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.

Member

v4hn commented Oct 16, 2016

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

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Oct 17, 2016

Contributor

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

Contributor

rhaschke commented Oct 17, 2016

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

@v4hn

This comment has been minimized.

Show comment
Hide comment
@v4hn

v4hn Oct 17, 2016

Member

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

Member

v4hn commented Oct 17, 2016

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

@rhaschke

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Oct 18, 2016

Contributor

Fixed by #290 and #292.

Contributor

rhaschke commented Oct 18, 2016

Fixed by #290 and #292.

@crvogt

This comment has been minimized.

Show comment
Hide comment
@crvogt

crvogt Aug 30, 2017

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 :)

crvogt commented Aug 30, 2017

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

This comment has been minimized.

Show comment
Hide comment
@v4hn

v4hn Aug 30, 2017

Member
Member

v4hn commented Aug 30, 2017

@crvogt

This comment has been minimized.

Show comment
Hide comment
@crvogt

crvogt Aug 30, 2017

@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 :)

crvogt commented Aug 30, 2017

@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 :)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment