Fix OrientationConstraint::decide (moveit#2414)

* Fix OrientationConstraint::decide

* Add new test case for OrientationConstraintsSimple

* Add comments for clarity

Author: stuart-fb

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

@@ -620,9 +620,10 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob
     xyz = diff.linear().eulerAngles(0, 1, 2);  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
   }
 
-  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
-  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
-  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
+  // Account for angle wrapping
+  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::two_pi<double>() - fabs(xyz(0)));
+  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::two_pi<double>() - fabs(xyz(1)));
+  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::two_pi<double>() - fabs(xyz(2)));
   bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
                 xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
                 xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();

moveit_core/kinematic_constraints/test/test_constraints.cpp

@@ -40,6 +40,7 @@
 #include <fstream>
 #include <tf2_eigen/tf2_eigen.h>
 #include <moveit/utils/robot_model_test_utils.h>
+#include <boost/math/constants/constants.hpp>
 
 class LoadPlanningModelsPr2 : public testing::Test
 {
@@ -652,6 +653,12 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
   robot_state.setVariablePositions(jvals);
   robot_state.update();
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
+
+  // rotation by pi does not wrap to zero
+  jvals["r_wrist_roll_joint"] = boost::math::constants::pi<double>();
+  robot_state.setVariablePositions(jvals);
+  robot_state.update();
+  EXPECT_FALSE(oc.decide(robot_state).satisfied);
 }
 
 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)