Ompl constrained orientation rebase

moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp

@@ -46,6 +46,21 @@
 class SphericalRobot : public testing::Test
 {
 protected:
+  /** A robot model with a single floating joint will be created. **/
+  moveit::core::RobotModelPtr robot_model_;
+
+  /** Test data
+   *
+   * The test data represents valid orientations for an absolute tolerance of 0.5 around x, y and z.
+   *
+   * The first matrix contains valid orientations for xyx_euler_angles error, but invalid for rotation_vector.
+   * The second matrix is the other way around.
+   *
+   * The rows contain a quaternion in the order x, y, z and w.
+   * */
+  Eigen::Matrix<double, 8, 4> valid_euler_data_;
+  Eigen::Matrix<double, 8, 4> valid_rotvec_data_;
+
   void SetUp() override
   {
     moveit::core::RobotModelBuilder builder("robot", "base_link");
@@ -72,7 +87,8 @@ class SphericalRobot : public testing::Test
   }
 
 protected:
-  moveit::core::RobotModelPtr robot_model_;
+  // moveit::core::RobotModelPtr robot_model_;
+  moveit::core::RobotModelPtr robot_wrist_model_;
 };
 
 class FloatingJointRobot : public testing::Test
@@ -325,27 +341,30 @@ TEST_F(SphericalRobot, Test4)
 TEST_F(SphericalRobot, Test5)
 {
   kinematic_constraints::OrientationConstraint oc(robot_model_);
+  EXPECT_TRUE(oc.configure(ocm, tf));
+  EXPECT_EQ(oc.getParameterization(), moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES);
+}
 
-  moveit::core::Transforms tf(robot_model_->getModelFrame());
-  moveit_msgs::OrientationConstraint ocm;
+moveit::core::Transforms tf(robot_model_->getModelFrame());
+moveit_msgs::OrientationConstraint ocm;
 
-  moveit::core::RobotState robot_state(robot_model_);
-  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.0));
-  robot_state.update();
+moveit::core::RobotState robot_state(robot_model_);
+robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.0));
+robot_state.update();
 
-  ocm.link_name = "yaw";
-  ocm.header.frame_id = robot_model_->getModelFrame();
-  ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear()));
-  ocm.absolute_x_axis_tolerance = 0.0;
-  ocm.absolute_y_axis_tolerance = 0.0;
-  ocm.absolute_z_axis_tolerance = 1.0;
-  ocm.weight = 1.0;
+ocm.link_name = "yaw";
+ocm.header.frame_id = robot_model_->getModelFrame();
+ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear()));
+ocm.absolute_x_axis_tolerance = 0.0;
+ocm.absolute_y_axis_tolerance = 0.0;
+ocm.absolute_z_axis_tolerance = 1.0;
+ocm.weight = 1.0;
 
-  robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi<double>(), 0.3));
-  robot_state.update();
+robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi<double>(), 0.3));
+robot_state.update();
 
-  EXPECT_TRUE(oc.configure(ocm, tf));
-  EXPECT_TRUE(oc.decide(robot_state, true).satisfied);
+EXPECT_TRUE(oc.configure(ocm, tf));
+EXPECT_TRUE(oc.decide(robot_state, true).satisfied);
 }
 
 TEST_F(FloatingJointRobot, TestDefaultParameterization)
@@ -474,6 +493,16 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization)
   EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
 }
 
+ocm.parameterization = moveit_msgs::OrientationConstraint::ROTATION_VECTOR;
+ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.3));
+EXPECT_TRUE(oc_rotvec.configure(ocm, tf));
+EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied);
+
+ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.6));
+EXPECT_TRUE(oc_rotvec.configure(ocm, tf));
+EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

moveit_planners/ompl/ompl_interface/CMakeLists.txt

@@ -54,7 +54,6 @@ if(CATKIN_ENABLE_TESTING)
 
   # Unfortunately it is not possible to test the planning context manager without introducing a ROS dependency.
   find_package(rostest REQUIRED)
-  find_package(tf2_eigen REQUIRED)
 
   add_rostest_gtest(test_planning_context_manager
     test/test_planning_context_manager.test
@@ -73,6 +72,10 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(test_constrained_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES})
   set_target_properties(test_constrained_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
+  catkin_add_gtest(test_threadsafe_state_storage test/test_threadsafe_state_storage.cpp)
+  target_link_libraries(test_threadsafe_state_storage ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES})
+  set_target_properties(test_threadsafe_state_storage PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
+
   catkin_add_gtest(test_ompl_constraints test/test_ompl_constraints.cpp)
   target_link_libraries(test_ompl_constraints ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES})
 

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h

@@ -325,6 +325,41 @@ class EqualityPositionConstraint : public BaseConstraint
   std::vector<bool> is_dim_constrained_;
 };
 
+/******************************************
+ * Orientation constraints
+ * ****************************************/
+/** \brief Orientation constraints parameterized using exponential coordinates.
+ *
+ * An orientation constraints is modelled as a deviation from a target orientation.
+ * The deviation is represented using exponential coordinates. A three element vector represents the rotation axis
+ * multiplied with the angle in radians around this axis.
+ *
+ *  R_error = R_end_effector ^ (-1) * R_target
+ *  R_error -> rotation angle and axis           (using Eigen3)
+ *  error = angle * axis                         (vector with three elements)
+ *
+ *  And then the constraints can be written as
+ *
+ *     - absolute_x_axis_tolerance / 2 < error[0] < absolute_x_axis_tolerance / 2
+ *     - absolute_y_axis_tolerance / 2 < error[1] < absolute_y_axis_tolerance / 2
+ *     - absolute_z_axis_tolerance / 2 < error[2] < absolute_z_axis_tolerance / 2
+ *
+ * **IMPORTANT** It is NOT how orientation error is handled in the default MoveIt constraint samplers, where XYZ
+ * intrinsic euler angles are used. Using exponential coordinates is analog to how orientation Error is calculated in
+ * the TrajOpt  motion planner.
+ *
+ * */
+class OrientationConstraint : public BaseConstraint
+{
+public:
+  OrientationConstraint(const robot_model::RobotModelConstPtr& robot_model, const std::string& group,
+                        const unsigned int num_dofs);
+
+  void parseConstraintMsg(const moveit_msgs::Constraints& constraints) override;
+  Eigen::VectorXd calcError(const Eigen::Ref<const Eigen::VectorXd>& x) const override;
+  Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref<const Eigen::VectorXd>& x) const override;
+};
+
 /** \brief Extract position constraints from the MoveIt message.
  *
  * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`.
@@ -333,9 +368,41 @@ class EqualityPositionConstraint : public BaseConstraint
  * */
 Bounds positionConstraintMsgToBoundVector(const moveit_msgs::PositionConstraint& pos_con);
 
+/** \brief Extract orientation constraints from the MoveIt message
+ *
+ * These bounds are assumed to be centered around the target orientation / desired orientation
+ * given in the "orientation" field in the message.
+ * These bounds represent orientation error between the desired orientation and the current orientation of the
+ * end-effector.
+ *
+ * The "absolute_x_axis_tolerance", "absolute_y_axis_tolerance" and "absolute_z_axis_tolerance" are interpreted as
+ * the width of the tolerance regions around the target orientation, represented using exponential coordinates.
+ *
+ * */
+Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con);
+
 /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/
 std::shared_ptr<BaseConstraint> createOMPLConstraint(const robot_model::RobotModelConstPtr& robot_model,
                                                      const std::string& group,
                                                      const moveit_msgs::Constraints& constraints);
 
+/** Convert anglular velocity to angle-axis velocity, base on:
+ * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
+ * */
+inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref<const Eigen::Vector3d> axis)
+{
+  // Eigen::Matrix3d E;
+
+  double t{ std::abs(angle) };
+  Eigen::Matrix3d r_skew;
+  r_skew << 0, -axis[2], axis[1], axis[2], 0, -axis[0], -axis[1], axis[0], 0;
+  r_skew *= angle;
+
+  double C;
+  C = (1 - 0.5 * t * std::sin(t) / (1 - std::cos(t)));
+
+  return Eigen::Matrix3d::Identity() - 0.5 * r_skew + r_skew * r_skew / (t * t) * C;
+  // return E;
+}
+
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

@@ -266,6 +266,55 @@ void EqualityPositionConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd
   }
 }
 
+/******************************************
+ * Orientation constraints
+ * ****************************************/
+OrientationConstraint::OrientationConstraint(const robot_model::RobotModelConstPtr& robot_model,
+                                             const std::string& group, const unsigned int num_dofs)
+  : BaseConstraint(robot_model, group, num_dofs)
+{
+}
+
+void OrientationConstraint::parseConstraintMsg(const moveit_msgs::Constraints& constraints)
+{
+  ROS_INFO_STREAM_NAMED(LOGNAME, "Parsing Orientation constraints for OMPL constrained state space.");
+  assert(bounds_.size() == 0);
+  bounds_ = orientationConstraintMsgToBoundVector(constraints.orientation_constraints.at(0));
+  ROS_INFO_NAMED(LOGNAME, "Parsed Orientation constraints");
+  ROS_INFO_STREAM_NAMED(LOGNAME, bounds_);
+  // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed rx / roll constraints" << bounds_[0]);
+  // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed ry / pitch constraints" << bounds_[1]);
+  // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed rz / yaw constraints" << bounds_[2]);
+
+  // extract target position and orientation
+  // geometry_msgs::Point position =
+  //     constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position;
+  // target_position_ << position.x, position.y, position.z;
+  tf2::fromMsg(constraints.orientation_constraints.at(0).orientation, target_orientation_);
+  ROS_INFO_NAMED(LOGNAME, "Quaternion desired");
+  ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.x());
+  ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.y());
+  ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.z());
+  ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.w());
+
+  link_name_ = constraints.orientation_constraints.at(0).link_name;
+  ROS_INFO_STREAM_NAMED(LOGNAME, "Orientation constraints applied to link: " << link_name_);
+}
+
+Eigen::VectorXd OrientationConstraint::calcError(const Eigen::Ref<const Eigen::VectorXd>& x) const
+{
+  Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_;
+  Eigen::AngleAxisd aa(orientation_difference);
+  return aa.axis() * aa.angle();
+}
+
+Eigen::MatrixXd OrientationConstraint::calcErrorJacobian(const Eigen::Ref<const Eigen::VectorXd>& x) const
+{
+  Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_;
+  Eigen::AngleAxisd aa{ orientation_difference };
+  return -angularVelocityToAngleAxis(aa.angle(), aa.axis()) * robotGeometricJacobian(x).bottomRows(3);
+}
+
 /************************************
  * MoveIt constraint message parsing
  * **********************************/
@@ -283,6 +332,21 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::PositionConstraint&
   return { { -dims[0] / 2, -dims[1] / 2, -dims[2] / 2 }, { dims[0] / 2, dims[1] / 2, dims[2] / 2 } };
 }
 
+Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con)
+{
+  std::vector<double> dims{ ori_con.absolute_x_axis_tolerance, ori_con.absolute_y_axis_tolerance,
+                            ori_con.absolute_z_axis_tolerance };
+
+  // dimension of -1 signifies unconstrained parameter, so set to infinity
+  for (auto& dim : dims)
+  {
+    if (dim == -1)
+      dim = std::numeric_limits<double>::infinity();
+  }
+  // return { { -dims[0], dims[0] }, { -dims[1], dims[1] }, { -dims[2], dims[2] } };
+  return { { -dims[0], -dims[1], -dims[2] }, { dims[0], dims[1], dims[2] } };
+}
+
 /******************************************
  * OMPL Constraints Factory
  * ****************************************/
@@ -331,8 +395,11 @@ std::shared_ptr<BaseConstraint> createOMPLConstraint(const robot_model::RobotMod
   }
   else if (num_ori_con > 0)
   {
-    ROS_ERROR_NAMED(LOGNAME, "Orientation constraints are not yet supported.");
-    return nullptr;
+    ROS_INFO_NAMED(LOGNAME, "OMPL is using orientation constraints.");
+    BaseConstraintPtr ori_con;
+    ori_con = std::make_shared<OrientationConstraint>(robot_model, group, num_dofs);
+    ori_con->init(constraints);
+    return ori_con;
   }
   else
   {

moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp

@@ -61,7 +61,6 @@ moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() cons
   if (it == thread_states_.end())
   {
     st = new moveit::core::RobotState(start_state_);
-    st->setToDefaultValues();  // avoid uninitialized memory
     thread_states_[std::this_thread::get_id()] = st;
   }
   else

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

@@ -528,7 +528,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
 
   if (constrained_planning_iterator != pc->second.config.end() &&
       boost::lexical_cast<bool>(constrained_planning_iterator->second) &&
-      req.path_constraints.position_constraints.size() == 1)
+      (req.path_constraints.position_constraints.size() == 1 ||
+       req.path_constraints.orientation_constraints.size() == 1))
   {
     factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1,
                                  ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE);