[WIP] Cartesian Tool Path Planning

moveit2.repos

@@ -1,9 +1,11 @@
 repositories:
   # Keep moveit_* repos here because they are released with moveit
+  # TODO: DO NOT MERGE WITH THIS! #
+  # Wait until https://github.com/ros-planning/moveit_msgs/pull/152 is merged
   moveit_msgs:
     type: git
-    url: https://github.com/ros-planning/moveit_msgs.git
-    version: ros2
+    url: https://github.com/sea-bass/moveit_msgs.git
+    version: pr-add-path-constraint
   moveit_resources:
     type: git
     url: https://github.com/ros-planning/moveit_resources.git

moveit_core/kinematic_constraints/src/utils.cpp

@@ -127,13 +127,17 @@ moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constrain
   for (const moveit_msgs::msg::VisibilityConstraint& visibility_constraint : second.visibility_constraints)
     r.visibility_constraints.push_back(visibility_constraint);
 
+  r.path_constraints = first.path_constraints;
+  for (const moveit_msgs::msg::PathConstraint& path_constraint : second.path_constraints)
+    r.path_constraints.push_back(path_constraint);
+
   return r;
 }
 
 std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr)
 {
   return constr.position_constraints.size() + constr.orientation_constraints.size() +
-         constr.visibility_constraints.size() + constr.joint_constraints.size();
+         constr.visibility_constraints.size() + constr.joint_constraints.size() + constr.path_constraints.size();
 }
 
 moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state,
@@ -548,6 +552,19 @@ static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::
   return true;
 }
 
+// Initialize a PathConstraint message from node parameters specified at constraint_param.
+static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::string& constraint_param,
+                                moveit_msgs::msg::PathConstraint& constraint)
+{
+  node->get_parameter(constraint_param + ".frame_id", constraint.header.frame_id);
+  node->get_parameter(constraint_param + ".weight", constraint.weight);
+  node->get_parameter(constraint_param + ".link_name", constraint.link_name);
+
+  // TODO: Implement this
+
+  return true;
+}
+
 // Initialize a Constraints message containing constraints specified by node parameters under constraint_ids.
 static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& constraint_ids,
                                moveit_msgs::msg::Constraints& constraints)
@@ -586,6 +603,13 @@ static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::v
       if (!constructConstraint(node, constraint_param, constraints.visibility_constraints.back()))
         return false;
     }
+    // TODO: Implement for Path Constraint
+    // else if (constraint_type == "path")
+    // {
+    //   constraints.path_constraints.emplace_back();
+    //   if (!constructConstraint(node, constraint_param, constraints.path_constraints.back()))
+    //     return false;
+    // }
     else
     {
       RCLCPP_ERROR_STREAM(LOGGER, "Unable to process unknown constraint type: " << constraint_type);

moveit_core/utils/src/message_checks.cpp

@@ -66,7 +66,7 @@ bool isEmpty(const moveit_msgs::msg::RobotState& msg)
 bool isEmpty(const moveit_msgs::msg::Constraints& constr)
 {
   return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
-         constr.visibility_constraints.empty() && constr.joint_constraints.empty();
+         constr.visibility_constraints.empty() && constr.joint_constraints.empty() && constr.path_constraints.empty();
 }
 
 bool isEmpty(const geometry_msgs::msg::Quaternion& msg)

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

@@ -39,11 +39,14 @@
 #include <ostream>
 
 #include <ompl/base/Constraint.h>
+#include <ompl/datastructures/GreedyKCenters.h>
+#include <ompl/datastructures/NearestNeighborsGNAT.h>
 
 #include <moveit/ompl_interface/detail/threadsafe_state_storage.h>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit_msgs/msg/constraints.hpp>
+#include <moveit_msgs/msg/path_constraint.hpp>
 
 namespace ompl_interface
 {
@@ -195,17 +198,17 @@ class BaseConstraint : public ompl::base::Constraint
 
   // the methods below are specifically for debugging and testing
 
-  const std::string& getLinkName()
+  const std::string& getLinkName() const
   {
     return link_name_;
   }
 
-  const Eigen::Vector3d getTargetPosition()
+  const Eigen::Vector3d getTargetPosition() const
   {
     return target_position_;
   }
 
-  const Eigen::Quaterniond getTargetOrientation()
+  const Eigen::Quaterniond getTargetOrientation() const
   {
     return target_orientation_;
   }
@@ -436,6 +439,10 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstr
  * */
 Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con);
 
+/** \brief TODO docstring
+ * */
+Bounds pathConstraintMsgToBoundVector(const moveit_msgs::msg::PathConstraint& path_con);
+
 /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/
 ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelConstPtr& robot_model,
                                                 const std::string& group,
@@ -459,4 +466,82 @@ inline Eigen::Matrix3d angularVelocityToAngleAxis(const double& angle, const Eig
   return Eigen::Matrix3d::Identity() - 0.5 * r_skew + (r_skew * r_skew / (t * t)) * c;
 }
 
+/******************************************
+ * Tool path constraint
+ * ****************************************/
+/** \brief Helper struct that wraps Eigen::Isometry3d to be used with OMPL's nearest neighbors implementation
+ */
+struct EigenIsometry3dWrapper
+{
+  EigenIsometry3dWrapper(){};
+  EigenIsometry3dWrapper(const Eigen::Isometry3d& tform, const size_t path_idx = 0)
+    : tform_{ tform }, path_idx_{ path_idx }
+  {
+  }
+
+  bool operator==(const EigenIsometry3dWrapper& other) const
+  {
+    return tform_.isApprox(other.tform_, 0.0001);
+  }
+
+  bool operator!=(const EigenIsometry3dWrapper& other) const
+  {
+    return !(tform_.isApprox(other.tform_, 0.0001));
+  }
+
+  size_t path_idx_ = 0;
+  Eigen::Isometry3d tform_;
+};
+
+/**
+ * @brief Find arc length between two quaternions
+ * @param pose1 - first pose
+ * @param pose2 - second pose
+ * @return Arc length
+ */
+double arcLength(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2);
+
+/**
+ * @brief Interpolate between two poses
+ * @param pose1 - first pose
+ * @param pose2 - second pose
+ * @param step - step from first pose to pose 2 [0 1]
+ * @return Interpolated pose
+ */
+Eigen::Isometry3d interpolatePose(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double step);
+
+/** \brief A Cartesian path constraint a specified link, useful for appliations such as sanding, painting, drawing,
+ * welding, etc.
+ * */
+class ToolPathConstraint : public BaseConstraint
+{
+public:
+  ToolPathConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group,
+                     unsigned int num_dofs, const unsigned int num_cons = 3);
+
+  void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override;
+
+  void setPath(const moveit_msgs::msg::PathConstraint& msg);
+
+  void function(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> out) const override;
+
+  void jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::MatrixXd> out) const override;
+
+private:
+  // Nearest-neighbors structure
+  mutable ompl::NearestNeighborsGNAT<EigenIsometry3dWrapper> pose_nn_;  // store poses
+
+  // Path representation
+  std::vector<EigenIsometry3dWrapper> path_;
+
+  // Interpolation distance for path
+  double interp_distance_;
+
+  // Data members (TODO document)
+  moveit::core::RobotModelConstPtr robot_model_;
+  std::string group_;
+  unsigned int num_dofs_;
+  std::vector<std::shared_ptr<BoxConstraint>> box_constraints_;
+};
+
 }  // namespace ompl_interface

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

@@ -152,4 +152,5 @@ class OMPLInterface
 private:
   constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
 };
+
 }  // namespace ompl_interface