Port OMPL orientation constraints to MoveIt2

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

@@ -604,7 +604,7 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra
 
   if (oc.weight <= std::numeric_limits<double>::epsilon())
   {
-    RCLCPP_WARN(LOGGER, "The weight on position constraint for link '%s' is near zero.  Setting to 1.0.",
+    RCLCPP_WARN(LOGGER, "The weight on orientation constraint for link '%s' is near zero.  Setting to 1.0.",
                 oc.link_name.c_str());
     constraint_weight_ = 1.0;
   }

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

@@ -260,10 +260,35 @@ class BoxConstraint : public BaseConstraint
   BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group,
                 const unsigned int num_dofs);
 
+  /** \brief Parse bounds on position parameters from MoveIt's constraint message.
+   *
+   * This can be non-trivial given the often complex structure of these messages.
+   * */
   void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override;
 
+  /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds.
+   *
+   * In this Position constraints case, it calculates the x, y and z position
+   * of the end-effector. This error is then converted in generic equality constraints in the implementation of
+   * `ompl_interface::BaseConstraint::function`.
+   *
+   * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore
+   * the bounds calculation.
+   * */
   Eigen::VectorXd calcError(const Eigen::Ref<const Eigen::VectorXd>& x) const override;
 
+  /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained.
+   *   *
+   * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error.
+   * It does not take into account the derivative of the penalty functions defined in the Bounds class.
+   * This correction is added in the implementation of of BaseConstraint::jacobian.
+   *
+   * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore
+   * the bounds calculation.
+   *
+   * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better
+   * performance?
+   * */
   Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref<const Eigen::VectorXd>& x) const override;
 };
 
@@ -287,6 +312,10 @@ class EqualityPositionConstraint : public BaseConstraint
   EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group,
                              const unsigned int num_dofs);
 
+  /** \brief Parse bounds on position parameters from MoveIt's constraint message.
+   *
+   * This can be non-trivial given the often complex structure of these messages.
+   * */
   void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override;
 
   void function(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> out) const override;
@@ -321,6 +350,71 @@ class EqualityPositionConstraint : public BaseConstraint
   std::vector<bool> is_dim_constrained_;
 };
 
+/******************************************
+ * Orientation constraints
+ * ****************************************/
+/** \brief Orientation constraints parameterized using exponential coordinates.
+ *
+ * An orientation constraint is modeled 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 moveit::core::RobotModelConstPtr& robot_model, const std::string& group,
+                        const unsigned int num_dofs)
+    : BaseConstraint(robot_model, group, num_dofs)
+  {
+  }
+
+  /** \brief Parse bounds on orientation parameters from MoveIt's constraint message.
+   *
+   * This can be non-trivial given the often complex structure of these messages.
+   * */
+  void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override;
+
+  /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds.
+   *
+   * In this orientation constraints case, it calculates the orientation
+   * of the end-effector. This error is then converted in generic equality constraints in the implementation of
+   * `ompl_interface::BaseConstraint::function`.
+   *
+   * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore
+   * the bounds calculation.
+   * */
+  Eigen::VectorXd calcError(const Eigen::Ref<const Eigen::VectorXd>& x) const override;
+
+  /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained.
+   *   *
+   * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error.
+   * It does not take into account the derivative of the penalty functions defined in the Bounds class.
+   * This correction is added in the implementation of of BaseConstraint::jacobian.
+   *
+   * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore
+   * the bounds calculation.
+   *
+   * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better
+   * performance?
+   * */
+  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`.
@@ -329,9 +423,40 @@ class EqualityPositionConstraint : public BaseConstraint
  * */
 Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::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::msg::OrientationConstraint& ori_con);
+
 /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/
 std::shared_ptr<BaseConstraint> createOMPLConstraint(const moveit::core::RobotModelConstPtr& robot_model,
                                                      const std::string& group,
                                                      const moveit_msgs::msg::Constraints& constraints);
 
+/** \brief  Return a matrix to convert angular velocity to angle-axis velocity
+ *  Based on:
+ * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
+ * Eq. 2.107
+ * */
+inline Eigen::Matrix3d angularVelocityToAngleAxis(const double& angle, const Eigen::Ref<const Eigen::Vector3d>& axis)
+{
+  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;
+}
+
 }  // namespace ompl_interface

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

@@ -200,11 +200,8 @@ BoxConstraint::BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model
 
 void BoxConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints)
 {
-  RCLCPP_DEBUG(LOGGER, "Parsing box position constraint for OMPL constrained state space.");
   assert(bounds_.size() == 0);
   bounds_ = positionConstraintMsgToBoundVector(constraints.position_constraints.at(0));
-  RCLCPP_DEBUG(LOGGER, "Parsed Box constraints");
-  RCLCPP_DEBUG_STREAM(LOGGER, "Bounds: " << bounds_);
 
   // extract target position and orientation
   geometry_msgs::msg::Point position =
@@ -216,7 +213,6 @@ void BoxConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& cons
                target_orientation_);
 
   link_name_ = constraints.position_constraints.at(0).link_name;
-  RCLCPP_DEBUG_STREAM(LOGGER, "Position constraints applied to link: " << link_name_);
 }
 
 Eigen::VectorXd BoxConstraint::calcError(const Eigen::Ref<const Eigen::VectorXd>& x) const
@@ -270,12 +266,7 @@ void EqualityPositionConstraint::parseConstraintMsg(const moveit_msgs::msg::Cons
   tf2::fromMsg(constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).orientation,
                target_orientation_);
 
-  RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on x-position? " << (is_dim_constrained_[0] ? "yes" : "no"));
-  RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on y-position? " << (is_dim_constrained_[1] ? "yes" : "no"));
-  RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on z-position? " << (is_dim_constrained_[2] ? "yes" : "no"));
-
   link_name_ = constraints.position_constraints.at(0).link_name;
-  RCLCPP_DEBUG_STREAM(LOGGER, "Position constraints applied to link: " << link_name_);
 }
 
 void EqualityPositionConstraint::function(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
@@ -310,6 +301,32 @@ void EqualityPositionConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd
   }
 }
 
+/******************************************
+ * Orientation constraints
+ * ****************************************/
+void OrientationConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints)
+{
+  bounds_ = orientationConstraintMsgToBoundVector(constraints.orientation_constraints.at(0));
+
+  tf2::fromMsg(constraints.orientation_constraints.at(0).orientation, target_orientation_);
+
+  link_name_ = constraints.orientation_constraints.at(0).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
  * **********************************/
@@ -330,6 +347,20 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstr
            { dims.at(0) / 2.0, dims.at(1) / 2.0, dims.at(2) / 2.0 } };
 }
 
+Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::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[1], -dims[2] }, { dims[0], dims[1], dims[2] } };
+}
+
 /******************************************
  * OMPL Constraints Factory
  * ****************************************/
@@ -348,40 +379,38 @@ std::shared_ptr<BaseConstraint> createOMPLConstraint(const moveit::core::RobotMo
 
   if (num_pos_con > 1)
   {
-    RCLCPP_WARN(LOGGER, "Only a single position constraints supported. Using the first one.");
+    RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one.");
   }
   if (num_ori_con > 1)
   {
-    RCLCPP_WARN(LOGGER, "Only a single orientation constraints supported. Using the first one.");
+    RCLCPP_WARN(LOGGER, "Only a single orientation constraint is supported. Using the first one.");
   }
 
   if (num_pos_con > 0 && num_ori_con > 0)
   {
-    RCLCPP_ERROR(LOGGER, "Combining position and orientation constraints not implemented yet for OMPL's constrained "
+    RCLCPP_ERROR(LOGGER, "Combining position and orientation constraints is not implemented yet for OMPL's constrained "
                          "state space.");
     return nullptr;
   }
   else if (num_pos_con > 0)
   {
-    RCLCPP_DEBUG_STREAM(LOGGER, "Constraint name: " << constraints.name);
     BaseConstraintPtr pos_con;
     if (constraints.name == "use_equality_constraints")
     {
-      RCLCPP_DEBUG(LOGGER, "OMPL is using equality position constraints.");
       pos_con = std::make_shared<EqualityPositionConstraint>(robot_model, group, num_dofs);
     }
     else
     {
-      RCLCPP_DEBUG(LOGGER, "OMPL is using box position constraints.");
       pos_con = std::make_shared<BoxConstraint>(robot_model, group, num_dofs);
     }
     pos_con->init(constraints);
     return pos_con;
   }
   else if (num_ori_con > 0)
   {
-    RCLCPP_ERROR(LOGGER, "Orientation constraints are not yet supported.");
-    return nullptr;
+    auto 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/planning_context_manager.cpp

@@ -536,11 +536,14 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext(
   auto constrained_planning_iterator = pc->second.config.find("enforce_constrained_state_space");
   auto joint_space_planning_iterator = pc->second.config.find("enforce_joint_model_state_space");
 
+  // Use ConstrainedPlanningStateSpace if there is exactly one position constraint or one orientation constraint
+  // Mixed constraints are not supported
   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 = getStateSpaceFactory(JointModelStateSpace::PARAMETERIZATION_TYPE);
+    factory = getStateSpaceFactory(ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE);
   }
   else if (joint_space_planning_iterator != pc->second.config.end() &&
            boost::lexical_cast<bool>(joint_space_planning_iterator->second))