Fix orientation constraints failing on satisfied configurations #2434

JafarAbdi · 2020-11-27T21:35:37Z

Description

After applying the following fix #2414 a pipeline I’m working on isn’t working anymore (with a bunch of errors about constraints not statisfied one of them is ros.moveit_planners_ompl.constrained_goal_sampler: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?)

The reason is, right now we're calculating the error rotation matrix and call eulerAngles from Eigen to find the angles’ errors which return them in the following ranges [0:pi]x[-pi:pi]x[-pi:pi] (link)

The issues are
1- As shown in the first test, despite the error is 1.57rad around Z-axis, eulerAngles will return x=3.139981, y=3.139841, z=1.570810 causing the constraint to fail
2- Another example in the second test 0.15, -pi/2, 0.15 eulerAngles will return 3.05343, -1.5708, 3.05343 which again will cause the constraint to fail (link)
3- This function isn’t stable around the identity rotation link#1 and link#2

Even if we use the function from unsupported/Eigen it will cause similar problems but in different orientations for example if the error rotation matrix correspond to pure rotation about Y with angles bigger than pi/2 the returned roll/yaw angles will cause a satisfied constraint to fails again

~~To fix this I used the Euler angles of the desired and the current orientations to find the angles’ errors~~

To fix this I did copy the implementation from unsupported/Eigen/EulerSystem.h since it’s not released yet which will return the angles in the following ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] and handled the singularities differently when we compare the error with the tolerances

~~TODO: I need to add more tests before merging this~~

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

…on to check if the constraint is satisfied

mamoll · 2020-11-27T22:24:14Z

@JeroenDM this is relevant for you, too.

mamoll

This is some nice detective work, @JafarAbdi!

JeroenDM · 2020-12-02T10:09:57Z

@JafarAbdi Wow, thank you for figuring this out!

Since #2414 was merged my tests for #2402 failed. I suspect you also found the solution to my problem :)
I used some hardcoded test data here, generated with a Python script. If you think this is a good approach I can add the test for Euler angles to this PR.

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

moveit_core/utils/src/robot_model_test_utils.cpp

codecov · 2020-12-05T00:32:17Z

Codecov Report

Merging #2434 (5b7371a) into master (eca2deb) will decrease coverage by 0.17%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2434      +/-   ##
==========================================
- Coverage   60.47%   60.30%   -0.16%     
==========================================
  Files         351      351              
  Lines       26434    26452      +18     
==========================================
- Hits        15982    15949      -33     
- Misses      10452    10503      +51

Impacted Files	Coverage Δ
...kinematic_constraints/src/kinematic_constraint.cpp	`74.63% <100.00%> (+0.40%)`	⬆️
moveit_core/utils/src/robot_model_test_utils.cpp	`74.42% <100.00%> (+2.33%)`	⬆️
moveit_ros/moveit_servo/src/pose_tracking.cpp	`46.72% <100.00%> (+0.36%)`	⬆️
...lace/include/moveit/pick_place/manipulation_plan.h	`75.00% <0.00%> (-25.00%)`	⬇️
...ipulation/pick_place/src/manipulation_pipeline.cpp	`73.41% <0.00%> (-14.89%)`	⬇️
..._interface/src/detail/constrained_goal_sampler.cpp	`72.50% <0.00%> (-10.00%)`	⬇️
moveit_ros/manipulation/pick_place/src/pick.cpp	`68.94% <0.00%> (-9.70%)`	⬇️
moveit_ros/manipulation/pick_place/src/place.cpp	`62.50% <0.00%> (-6.94%)`	⬇️
...ion/pick_place/src/reachable_valid_pose_filter.cpp	`84.00% <0.00%> (-6.00%)`	⬇️
...eit_ros/manipulation/pick_place/src/pick_place.cpp	`88.89% <0.00%> (-5.55%)`	⬇️
... and 9 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 712a73a...5b7371a. Read the comment docs.

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

JeroenDM · 2020-12-07T10:28:56Z

+1 For explicitly handling and explaining the singular case, great work! @JafarAbdi

I was thinking the following case could maybe surprise some users:

Orientation tolerance of (0, 0, 1) on roll-pitch-yaw angles.
Robot at the singular case with theta = 0.5.
In this case, the yaw tolerance is not violated by theta, and therefore theta will be interpreted as pure roll.
This results in roll tolerance being violated.

Is this a correct interpretation?

Edit:
I don't know how often this singular case is relevant in practical problems, but if my interpretation is correct, an alternative could be to check theta against the maximum of the roll and yaw tolerance.

JafarAbdi · 2020-12-07T12:39:47Z

I used some hardcoded test data here, generated with a Python script. If you think this is a good approach I can add the test for Euler angles to this PR.

Sorry, I missed this comment, I'm okay with adding them here, or if you prefer I could open my PR against your branch to merge them together

JafarAbdi · 2020-12-07T12:44:09Z

I was thinking the following case could maybe surprise some users:

Orientation tolerance of (0, 0, 1) on roll-pitch-yaw angles.

Robot at the singular case with theta = 0.5.
In this case, the yaw tolerance is not violated by theta, and therefore theta will be interpreted as pure roll.
This results in roll tolerance being violated.

Is this a correct interpretation?

Edit:
I don't know how often this singular case is relevant in practical problems, but if my interpretation is correct, an alternative could be to check theta against the maximum of the roll and yaw tolerance.

As you can see in the image below given the tolerances this should pass, I added it as a test case

Also, I forgot to mention that in the last commits I reverted my previous approach and used the error rotation matrix to check for tolerances since it's easier to handle singularities

Co-authored-by: Jeroen <jeroendemaeyer@live.be>

JeroenDM · 2020-12-07T13:25:31Z

moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp

+  robot_state.update();
+  EXPECT_TRUE(oc.configure(ocm, tf));
+  EXPECT_FALSE(oc.decide(robot_state).satisfied);
+


Suggested change

// In the non-singular case the absolute_x_axis_tolerance is not violated

robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>() - 0.01, 0.21));

robot_state.update();

EXPECT_TRUE(oc.configure(ocm, tf));

EXPECT_TRUE(oc.decide(robot_state).satisfied);

JeroenDM · 2020-12-07T13:29:46Z

Also, I forgot to mention that in the last commits I reverted my previous approach and used the error rotation matrix to check for tolerances since it's easier to handle singularities

Ah, ok, now I understand it :) I glanced over that change :s
I suggested an extra test case to clarify the behaviour.

Sorry, I missed this comment, I'm okay with adding them here, or if you prefer I could open my PR against your branch to merge them together

Given you already wrote extensive tests now, I don't' think my other tests would add value here. I will just extend your test file for the new parameterization once this is merged.

JeroenDM · 2020-12-07T13:39:10Z

moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp

+  ocm.weight = 1.0;
+
+  moveit::core::RobotState robot_state(robot_model_);
+  // Singularity: roll + yaw = theta


Suggested change

// Singularity: roll + yaw = theta

// Singularity: roll - yaw = theta

Because the tests below use -pi / 2.

JeroenDM · 2020-12-07T13:40:53Z

Also, next time I will properly organize my comments in a review... sorry.

AndyZe · 2020-12-07T14:13:21Z

I just want to make sure you have especially tested several values around pitch = pi/2. That's where singularities occur for the Euler angle roll/pitch/yaw convention, I believe. See here, page 13.

AndyZe

~~Overall I think a better way to do this would be to convert to a rotation vector because it is unique and does not have singularities.~~

Never mind, that doesn't help because orientation constraints are specified in RPY.

Looks good to me!

AndyZe · 2020-12-08T14:21:45Z

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

@@ -58,6 +58,58 @@ static double normalizeAngle(double angle)
  return v;
 }

+// Normalizes an angle to the interval [-pi, +pi] and then take the absolute value
+// The returned values will be in the following range [0, +pi]


Minor correction, just to get the order of operations straight:

// Normalizes the absolute value of an angle to [0, +pi]
// Returned values will be in the range [0, +pi]

rhaschke

Looks reasonable. Will fix #2449 as well.
However, there is a failing (ikfast) test here. Not sure that is related. I retriggered Travis.

AndyZe · 2020-12-08T15:19:10Z

Let's run CI one more time

AndyZe · 2020-12-09T13:58:55Z

I should have squash-merged :/

* Add tests that trigger the bug * Use the angle differece between the desired and the current orientation to check if the constraint is satisfied * Handle singularities

rhaschke · 2020-12-09T22:31:50Z

I should have squash-merged :/

Yes, indeed. I cleaned up and force-pushed. Please update your devel branches @tylerjw, @AndyZe, @JafarAbdi.

rhaschke · 2020-12-09T22:59:30Z

@AndyZe: You pushed a branch revert-2434-pr-fix_orientation_constraints to the main repo. Was that intended to revert the accidental rebase merge? That would have made it even worse (adding even more commits...) If that branch is not needed anymore, please delete it.

AndyZe · 2020-12-10T03:47:56Z

done!

JafarAbdi added 3 commits November 27, 2020 23:18

robot_model_test_utils: Add parameter to specify the joint rotation axis

e93ddb4

Add tests the trigger the bug

74b7097

Use the angle differece between the desired and the current orientati…

7227d51

…on to check if the constraint is satisfied

JafarAbdi requested review from mlautman and rhaschke as code owners November 27, 2020 21:35

ClangFormat

367ffa5

mamoll approved these changes Nov 30, 2020

View reviewed changes

JeroenDM reviewed Dec 2, 2020

View reviewed changes

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp Outdated Show resolved Hide resolved

JeroenDM reviewed Dec 2, 2020

View reviewed changes

moveit_core/utils/src/robot_model_test_utils.cpp Show resolved Hide resolved

Fix singularities

1e7b454

JafarAbdi force-pushed the pr-fix_orientation_constraints branch 2 times, most recently from ad5d69f to 2c703b9 Compare December 4, 2020 21:29

Add unit tests

d3b5147

JafarAbdi force-pushed the pr-fix_orientation_constraints branch from 2c703b9 to d3b5147 Compare December 4, 2020 21:33

JeroenDM reviewed Dec 7, 2020

View reviewed changes

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp Outdated Show resolved Hide resolved

JafarAbdi and others added 3 commits December 7, 2020 15:44

Update moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

819d73e

Co-authored-by: Jeroen <jeroendemaeyer@live.be>

Add unit test

5fb6d15

Cleanup

5b7371a

JeroenDM reviewed Dec 7, 2020

View reviewed changes

JeroenDM approved these changes Dec 7, 2020

View reviewed changes

JeroenDM reviewed Dec 7, 2020

View reviewed changes

AndyZe mentioned this pull request Dec 8, 2020

pick place flaky test #2449

Closed

AndyZe approved these changes Dec 8, 2020

View reviewed changes

rhaschke reviewed Dec 8, 2020

View reviewed changes

AndyZe closed this Dec 8, 2020

AndyZe reopened this Dec 8, 2020

This comment has been minimized.

Sign in to view

felixvd mentioned this pull request Dec 9, 2020

setGoalTolerance() does not work in ros-noetic the way it worked in melodic. #2447

Open

AndyZe merged commit ba461a9 into moveit:master Dec 9, 2020

gautz mentioned this pull request Apr 9, 2021

Ompl constrained orientation JeroenDM/moveit#9

Draft

6 tasks

tylerjw mentioned this pull request Apr 9, 2021

Noetic Release 1.1.2 #2593

Closed

tylerjw pushed a commit to tylerjw/moveit that referenced this pull request Apr 29, 2021

Fix validation of orientation constraints (moveit#2434)

3a80122

tylerjw mentioned this pull request Apr 29, 2021

Melodic Backports #2630

Merged

tylerjw pushed a commit to tylerjw/moveit that referenced this pull request May 3, 2021

Fix validation of orientation constraints (moveit#2434)

82677a3

tylerjw pushed a commit that referenced this pull request May 3, 2021

Fix validation of orientation constraints (#2434)

c4f8fd4

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix orientation constraints failing on satisfied configurations #2434

Fix orientation constraints failing on satisfied configurations #2434

JafarAbdi commented Nov 27, 2020 •

edited

mamoll commented Nov 27, 2020

mamoll left a comment

JeroenDM commented Dec 2, 2020

codecov bot commented Dec 5, 2020 •

edited

JeroenDM commented Dec 7, 2020 •

edited

JafarAbdi commented Dec 7, 2020

JafarAbdi commented Dec 7, 2020

JeroenDM Dec 7, 2020

JeroenDM commented Dec 7, 2020

JeroenDM Dec 7, 2020

JeroenDM Dec 7, 2020

JeroenDM commented Dec 7, 2020

AndyZe commented Dec 7, 2020 •

edited

AndyZe left a comment •

edited

AndyZe Dec 8, 2020 •

edited

rhaschke left a comment

AndyZe commented Dec 8, 2020

This comment has been minimized.

AndyZe commented Dec 9, 2020

rhaschke commented Dec 9, 2020

rhaschke commented Dec 9, 2020

AndyZe commented Dec 10, 2020

+  // In the non-singular case the absolute_x_axis_tolerance is not violated
+  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>() - 0.01, 0.21));
+  robot_state.update();
+  EXPECT_TRUE(oc.configure(ocm, tf));
+  EXPECT_TRUE(oc.decide(robot_state).satisfied);

	// Singularity: roll + yaw = theta
	// Singularity: roll - yaw = theta

Fix orientation constraints failing on satisfied configurations #2434

Fix orientation constraints failing on satisfied configurations #2434

Conversation

JafarAbdi commented Nov 27, 2020 • edited

Description

Checklist

mamoll commented Nov 27, 2020

mamoll left a comment

Choose a reason for hiding this comment

JeroenDM commented Dec 2, 2020

codecov bot commented Dec 5, 2020 • edited

Codecov Report

JeroenDM commented Dec 7, 2020 • edited

JafarAbdi commented Dec 7, 2020

JafarAbdi commented Dec 7, 2020

JeroenDM Dec 7, 2020

Choose a reason for hiding this comment

JeroenDM commented Dec 7, 2020

JeroenDM Dec 7, 2020

Choose a reason for hiding this comment

JeroenDM Dec 7, 2020

Choose a reason for hiding this comment

JeroenDM commented Dec 7, 2020

AndyZe commented Dec 7, 2020 • edited

AndyZe left a comment • edited

Choose a reason for hiding this comment

AndyZe Dec 8, 2020 • edited

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

AndyZe commented Dec 8, 2020

This comment has been minimized.

AndyZe commented Dec 9, 2020

rhaschke commented Dec 9, 2020

rhaschke commented Dec 9, 2020

AndyZe commented Dec 10, 2020

JafarAbdi commented Nov 27, 2020 •

edited

codecov bot commented Dec 5, 2020 •

edited

JeroenDM commented Dec 7, 2020 •

edited

AndyZe commented Dec 7, 2020 •

edited

AndyZe left a comment •

edited

AndyZe Dec 8, 2020 •

edited