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 58bdda9a3a..06b8e07940 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 @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Jeroen De Maeyer, Boston Cleek */ +/* Author: Jeroen De Maeyer, Boston Cleek, Berend Pijnenburg */ #pragma once @@ -68,6 +68,7 @@ class Bounds { public: Bounds(); + Bounds(const std::vector& lower, const std::vector& upper); /** \brief Distance to region inside bounds * * Distance of a given value outside the bounds, zero inside the bounds. @@ -116,30 +117,21 @@ std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds) class BaseConstraint : public ompl::base::Constraint { public: - /** \brief Construct a BaseConstraint using 3 `num_cons` by default because all constraints currently implemented have - * 3 constraint equations. **/ BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs, const unsigned int num_cons = 3); - - /** \brief Initialize constraint based on message content. - * - * This is necessary because we cannot call the pure virtual - * parseConstraintsMsg method from the constructor of this class. - * */ - void init(const moveit_msgs::msg::Constraints& constraints); + const unsigned int num_dofs, const unsigned int num_cons); /** OMPL's main constraint evaluation function. * * OMPL requires you to override at least "function" which represents the constraint F(q) = 0 * */ - void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + virtual void function(const Eigen::Ref& joint_values, Eigen::Ref out) const = 0; /** \brief Jacobian of the constraint function. * * Optionally you can also provide dF(q)/dq, the Jacobian of the constraint. * * */ - void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + virtual void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const = 0; /** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State. * @@ -157,37 +149,6 @@ class BaseConstraint : public ompl::base::Constraint * */ Eigen::MatrixXd robotGeometricJacobian(const Eigen::Ref& joint_values) const; - /** \brief Parse bounds on position and orientation parameters from MoveIt's constraint message. - * - * This can be non-trivial given the often complex structure of these messages. - * */ - virtual void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) = 0; - - /** \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. - * */ - virtual Eigen::VectorXd calcError(const Eigen::Ref& /*x*/) const; - - /** \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? - * */ - virtual Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& /*x*/) const; - // the methods below are specifically for debugging and testing const std::string& getLinkName() @@ -325,44 +286,17 @@ 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) - { - } + const unsigned int num_dofs, const moveit_msgs::msg::OrientationConstraint& ori_con); - /** \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; + void function(const Eigen::Ref& joint_values, Eigen::Ref out) const 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& x) const override; + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) 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& x) const override; -}; +private: + + std::vector getConstrainedDims(const moveit_msgs::msg::OrientationConstraint& ori_con) const; -/** \brief Extract orientation constraints from the MoveIt message +/** \brief * * These bounds are assumed to be centered around the target orientation / desired orientation * given in the "orientation" field in the message. @@ -373,7 +307,13 @@ class OrientationConstraint : public BaseConstraint * the width of the tolerance regions around the target orientation, represented using exponential coordinates. * * */ -Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con); + Bounds createBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con, const std::vector& constrained_dims) const; + + std::vector constrained_dims_; +}; + + + /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelConstPtr& robot_model, 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 892c50df48..3e0c0f44fc 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Jeroen De Maeyer, Boston Cleek */ +/* Author: Jeroen De Maeyer, Boston Cleek, Berend Pijnenburg */ #include #include @@ -105,37 +105,14 @@ std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds) * Base class for constraints * **************************/ BaseConstraint::BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs, const unsigned int num_cons_) - : ompl::base::Constraint(num_dofs, num_cons_) + const unsigned int num_dofs, const unsigned int num_cons) + : ompl::base::Constraint(num_dofs, num_cons) , state_storage_(robot_model) , joint_model_group_(robot_model->getJointModelGroup(group)) { } -void BaseConstraint::init(const moveit_msgs::msg::Constraints& constraints) -{ - parseConstraintMsg(constraints); -} - -void BaseConstraint::function(const Eigen::Ref& joint_values, - Eigen::Ref out) const -{ - const Eigen::VectorXd current_values = calcError(joint_values); - out = bounds_.penalty(current_values); -} - -void BaseConstraint::jacobian(const Eigen::Ref& joint_values, - Eigen::Ref out) const -{ - const Eigen::VectorXd constraint_error = calcError(joint_values); - const Eigen::MatrixXd robot_jacobian = calcErrorJacobian(joint_values); - for (std::size_t i = 0; i < bounds_.size(); ++i) - { - out.row(i) = robot_jacobian.row(i); - } -} - Eigen::Isometry3d BaseConstraint::forwardKinematics(const Eigen::Ref& joint_values) const { moveit::core::RobotState* robot_state = state_storage_.getStateStorage(); @@ -154,20 +131,6 @@ Eigen::MatrixXd BaseConstraint::robotGeometricJacobian(const Eigen::Ref& /*x*/) const -{ - RCLCPP_WARN_STREAM(getLogger(), - "BaseConstraint: Constraint method calcError was not overridden, so it should not be used."); - return Eigen::VectorXd::Zero(getCoDimension()); -} - -Eigen::MatrixXd BaseConstraint::calcErrorJacobian(const Eigen::Ref& /*x*/) const -{ - RCLCPP_WARN_STREAM( - getLogger(), "BaseConstraint: Constraint method calcErrorJacobian was not overridden, so it should not be used."); - return Eigen::MatrixXd::Zero(getCoDimension(), n_); -} - /****************************************** * Position constraints * ****************************************/ @@ -262,45 +225,96 @@ Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstrai /****************************************** * Orientation constraints * ****************************************/ -void OrientationConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) +OrientationConstraint::OrientationConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs, const moveit_msgs::msg::OrientationConstraint& ori_con) + : BaseConstraint(robot_model, group, num_dofs, 0) { - bounds_ = orientationConstraintMsgToBoundVector(constraints.orientation_constraints.at(0)); + constrained_dims_ = getConstrainedDims(ori_con); - tf2::fromMsg(constraints.orientation_constraints.at(0).orientation, target_orientation_); + setManifoldDimension(num_dofs - constrained_dims_.size()); - link_name_ = constraints.orientation_constraints.at(0).link_name; + bounds_ = createBoundVector(ori_con, constrained_dims_); + + tf2::fromMsg(ori_con.orientation, target_orientation_); + + link_name_ = ori_con.link_name; } -Eigen::VectorXd OrientationConstraint::calcError(const Eigen::Ref& x) const +void OrientationConstraint::function(const Eigen::Ref& joint_values, Eigen::Ref out) const { - Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_; + Eigen::Matrix3d orientation_difference = forwardKinematics(joint_values).linear().transpose() * target_orientation_; Eigen::AngleAxisd aa(orientation_difference); - return aa.axis() * aa.angle(); + Eigen::VectorXd error = aa.axis() * aa.angle(); + + int emplace_index = 0; + for (auto& dim : constrained_dims_) + { + out[emplace_index] = error[dim]; + emplace_index++; + } + out = bounds_.penalty(out); } -Eigen::MatrixXd OrientationConstraint::calcErrorJacobian(const Eigen::Ref& x) const +void OrientationConstraint::jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const { - Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_; + Eigen::Matrix3d orientation_difference = forwardKinematics(joint_values).linear().transpose() * target_orientation_; Eigen::AngleAxisd aa{ orientation_difference }; - return -angularVelocityToAngleAxis(aa.angle(), aa.axis()) * robotGeometricJacobian(x).bottomRows(3); + Eigen::MatrixXd jac = -angularVelocityToAngleAxis(aa.angle(), aa.axis()) * robotGeometricJacobian(joint_values).bottomRows(3); + + int emplace_index = 0; + for (auto& dim : constrained_dims_) + { + out.row(emplace_index) = jac.row(dim); + emplace_index++; + } } -/************************************ - * MoveIt constraint message parsing - * **********************************/ +std::vector OrientationConstraint::getConstrainedDims(const moveit_msgs::msg::OrientationConstraint& ori_con) const +{ + std::vector constrained_dims; + // If a tolerance is < 0 or infinity dont constrain it -Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con) + if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_x_axis_tolerance != std::numeric_limits::infinity()) + { + constrained_dims.push_back(0); + } + if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_y_axis_tolerance != std::numeric_limits::infinity()) + { + constrained_dims.push_back(1); + } + if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_z_axis_tolerance != std::numeric_limits::infinity()) + { + constrained_dims.push_back(2); + } + + return constrained_dims; +} + +Bounds OrientationConstraint::createBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con, const std::vector& constrained_dims) const { - std::vector dims = { ori_con.absolute_x_axis_tolerance * 2.0, ori_con.absolute_y_axis_tolerance * 2.0, - ori_con.absolute_z_axis_tolerance * 2.0 }; + std::vector lower; + std::vector upper; - // dimension of -1 signifies unconstrained parameter, so set to infinity - for (auto& dim : dims) + for (auto& dim : constrained_dims) { - if (dim == -1) - dim = std::numeric_limits::infinity(); + if (dim == 0) + { + lower.push_back(-ori_con.absolute_x_axis_tolerance); + upper.push_back(ori_con.absolute_x_axis_tolerance); + } + else if (dim == 1) + { + lower.push_back(-ori_con.absolute_y_axis_tolerance); + upper.push_back(ori_con.absolute_y_axis_tolerance); + } + else if (dim == 2) + { + lower.push_back(-ori_con.absolute_z_axis_tolerance); + upper.push_back(ori_con.absolute_z_axis_tolerance); + } + } - return { { -dims[0], -dims[1], -dims[2] }, { dims[0], dims[1], dims[2] } }; + return { lower, upper }; } /****************************************** @@ -348,8 +362,7 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo RCLCPP_WARN(getLogger(), "Only a single orientation constraint is supported. Using the first one."); } - auto ori_con = std::make_shared(robot_model, group, num_dofs); - ori_con->init(constraints); + auto ori_con = std::make_shared(robot_model, group, num_dofs, constraints.orientation_constraints.at(0)); ompl_constraints.emplace_back(ori_con); }