diff --git a/moveit2.repos b/moveit2.repos index 866f7ccdc7..4281065cb4 100644 --- a/moveit2.repos +++ b/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 diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h index ebbaa85276..7e53233dd4 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h @@ -39,11 +39,14 @@ #include #include +#include +#include #include #include #include #include +#include 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,83 @@ 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_, EQUALITY_CONSTRAINT_THRESHOLD); + } + + bool operator!=(const EigenIsometry3dWrapper& other) const + { + return !(tform_.isApprox(other.tform_, EQUALITY_CONSTRAINT_THRESHOLD)); + } + + size_t path_idx_ = 0; + Eigen::Isometry3d tform_; + static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; +}; + +/** + * @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& joint_values, Eigen::Ref out) const override; + + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + +private: + // Nearest-neighbors structure + mutable ompl::NearestNeighborsGNAT pose_nn_; // store poses + + // Path representation + std::vector 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> box_constraints_; +}; + } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index 9ae1ee16fa..a3bb436e0e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/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 diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index d7cce130b4..c427a7cd69 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -323,6 +323,167 @@ Eigen::MatrixXd OrientationConstraint::calcErrorJacobian(const Eigen::Ref 1) + { + path_.reserve(num_pts); + for (size_t i = 0; i < num_pts; i++) + { + const auto pose = path_constraint.path.at(i); + Eigen::Isometry3d pose_eigen; + tf2::fromMsg(pose, pose_eigen); + path_.emplace_back(EigenIsometry3dWrapper(pose_eigen, std::min(i, num_pts - 2))); + } + + // Add box constraints + for (size_t i = 0; i < num_pts - 1; i++) + { + const auto pose = path_.at(i).tform_; + const auto next_pose = path_.at(i + 1).tform_; + + auto pos_con = std::make_shared(robot_model_, group_, num_dofs_); + moveit_msgs::msg::PositionConstraint box_constraint; + box_constraint.header = path_constraint.header; + box_constraint.link_name = path_constraint.link_name; + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + box.dimensions = { 0.05, (pose.translation() - next_pose.translation()).norm() + 0.05, + 0.005 }; // TODO: Set proper dimensions + box_constraint.constraint_region.primitives.push_back(box); + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = 0.5 * (pose.translation().x() + next_pose.translation().x()); + box_pose.position.y = 0.5 * (pose.translation().y() + next_pose.translation().y()); + box_pose.position.z = 0.5 * (pose.translation().z() + next_pose.translation().z()); + box_pose.orientation.w = 1.0; // TODO: Orient the box + box_constraint.constraint_region.primitive_poses.push_back(box_pose); + box_constraint.weight = 1.0; + + moveit_msgs::msg::Constraints constr; + constr.position_constraints.push_back(box_constraint); + + pos_con->init(constr); + box_constraints_.emplace_back(pos_con); + // std::cout << "Made box constraint index " << i << std::endl; + } + } + else + { + RCLCPP_ERROR_STREAM(LOGGER, "Path constraint must specify at least 2 points!"); + return; + } + + // Extract the interpolation distance + if (path_constraint.interp_distance > 0.0) + { + interp_distance_ = path_constraint.interp_distance; + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Cannot use zero interpolation distance. Defaulting to 0.01."); + interp_distance_ = 0.01; + } + + // Interpolate along waypoints and insert into pose_nn_; + for (size_t i = 0; i < num_pts - 1; i++) + { + // Insert waypoints + pose_nn_.add(path_[i]); + + // TODO: Consider arc length between the poses to determine the number of interpolation steps + // Linearly determine the number of steps + const auto path_length = (path_[i].tform_.translation() - path_[i + 1].tform_.translation()).norm(); + const auto num_steps = static_cast(path_length / interp_distance_); + + // Insert interpolated poses + auto delta = interp_distance_; + for (size_t j = 0; j < num_steps - 1; j++) + { + const auto pose = interpolatePose(path_[i].tform_, path_[i + 1].tform_, delta); + pose_nn_.add(EigenIsometry3dWrapper(pose, i)); + delta += interp_distance_; + } + } + + // Insert last waypoint + pose_nn_.add(path_[num_pts - 1]); +} + +void ToolPathConstraint::function(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values); + + const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose)); + // std::cout << "Function, nearest: " << eef_nearest.path_idx_ << std::endl; + + const auto bc = box_constraints_.at(eef_nearest.path_idx_); + bc->function(joint_values, out); +} + +void ToolPathConstraint::jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values); + const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose)); + // std::cout << "Jacobian, nearest: " << eef_nearest.path_idx_ << std::endl; + + const auto bc = box_constraints_.at(eef_nearest.path_idx_); + bc->jacobian(joint_values, out); +} + +double arcLength(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2) +{ + const auto q1 = Eigen::Quaterniond(pose1.rotation()); + const auto q2 = Eigen::Quaterniond(pose2.rotation()); + return q1.angularDistance(q2); +} + +Eigen::Isometry3d interpolatePose(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double step) +{ + const Eigen::Translation3d interp_translation = + Eigen::Translation3d((1.0 - step) * pose1.translation() + step * pose2.translation()); + + const auto q1 = Eigen::Quaterniond(pose1.rotation()); + const auto q2 = Eigen::Quaterniond(pose2.rotation()); + const Eigen::Quaterniond interp_orientation = q1.slerp(step, q2); + + return interp_translation * interp_orientation; +} + /************************************ * MoveIt constraint message parsing * **********************************/ @@ -365,12 +526,12 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo const moveit_msgs::msg::Constraints& constraints) { // TODO(bostoncleek): does this reach the end w/o a return ? - const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount(); const std::size_t num_pos_con = constraints.position_constraints.size(); const std::size_t num_ori_con = constraints.orientation_constraints.size(); + const std::size_t num_path_con = constraints.path_constraints.size(); - // This factory method contains template code to support position and/or orientation constraints. + // This factory method contains template code to support constraints. // If the specified constraints are invalid, a nullptr is returned. std::vector ompl_constraints; if (num_pos_con > 1) @@ -381,6 +542,11 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo { RCLCPP_WARN(LOGGER, "Only a single orientation constraint is supported. Using the first one."); } + if (num_path_con > 1) + { + RCLCPP_WARN(LOGGER, "Only a single path constraint is supported. Using the first one."); + } + if (num_pos_con > 0) { BaseConstraintPtr pos_con; @@ -401,11 +567,19 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo ori_con->init(constraints); ompl_constraints.emplace_back(ori_con); } - if (num_pos_con < 1 && num_ori_con < 1) + if (num_path_con > 0) { - RCLCPP_ERROR(LOGGER, "No path constraints found in planning request."); + auto path_con = std::make_shared(robot_model, group, num_dofs); + path_con->init(constraints); + ompl_constraints.emplace_back(path_con); + } + + if (num_pos_con < 1 && num_ori_con < 1 && num_path_con < 1) + { + RCLCPP_ERROR(LOGGER, "No constraints found in planning request."); return nullptr; } return std::make_shared(num_dofs, ompl_constraints); } + } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index ce32c4e18e..3d84cc7d21 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -538,11 +538,12 @@ 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 and/or one orientation constraint + // Use ConstrainedPlanningStateSpace if there is exactly one position, orientation, and/or path constraint if (constrained_planning_iterator != pc->second.config.end() && boost::lexical_cast(constrained_planning_iterator->second) && ((req.path_constraints.position_constraints.size() == 1) || - (req.path_constraints.orientation_constraints.size() == 1))) + (req.path_constraints.orientation_constraints.size() == 1) || + (req.path_constraints.path_constraints.size() == 1))) { factory = getStateSpaceFactory(ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE); }