From f615595eb73ce66487bd6eef74cd9abfec88b8bf Mon Sep 17 00:00:00 2001 From: BerendPijnenburg Date: Thu, 28 Mar 2024 13:38:44 +0100 Subject: [PATCH 1/4] Removed redundant derivative function --- .../ompl_interface/detail/ompl_constraints.h | 23 ++++++--------- .../src/detail/ompl_constraints.cpp | 28 ++----------------- 2 files changed, 11 insertions(+), 40 deletions(-) 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 93b2cc8bc2..fb47d60b4e 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 @@ -68,30 +68,25 @@ 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. + * With the sign showing the direction of the penalty, * Creates a penalty function that looks like this: * * (penalty) ^ - * | \ / - * | \ / - * | \_____/ - * |----------------> (variable to be constrained) + * | / + * | / + * |--- _____/-------------> (variable to be constrained) + * | / + * | / + * | / + * v * */ Eigen::VectorXd penalty(const Eigen::Ref& x) const; - /** \brief Derivative of the penalty function - * ^ - * | - * | -1-1-1 0 0 0 +1+1+1 - * |------------------------> - * **/ - Eigen::VectorXd derivative(const Eigen::Ref& x) const; - std::size_t size() const; - + private: std::vector lower_, upper_; std::size_t size_; 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 a6bb82bde0..0c59e4fb54 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -72,7 +72,7 @@ Eigen::VectorXd Bounds::penalty(const Eigen::Ref& x) cons { if (x[i] < lower_.at(i)) { - penalty[i] = lower_.at(i) - x[i]; + penalty[i] = x[i] - lower_.at(i); } else if (x[i] > upper_.at(i)) { @@ -86,29 +86,6 @@ Eigen::VectorXd Bounds::penalty(const Eigen::Ref& x) cons return penalty; } -Eigen::VectorXd Bounds::derivative(const Eigen::Ref& x) const -{ - assert(static_cast(lower_.size()) == x.size()); - Eigen::VectorXd derivative(x.size()); - - for (unsigned int i = 0; i < x.size(); ++i) - { - if (x[i] < lower_.at(i)) - { - derivative[i] = -1.0; - } - else if (x[i] > upper_.at(i)) - { - derivative[i] = 1.0; - } - else - { - derivative[i] = 0.0; - } - } - return derivative; -} - std::size_t Bounds::size() const { return size_; @@ -152,11 +129,10 @@ void BaseConstraint::jacobian(const Eigen::Ref& joint_val Eigen::Ref out) const { const Eigen::VectorXd constraint_error = calcError(joint_values); - const Eigen::VectorXd constraint_derivative = bounds_.derivative(constraint_error); const Eigen::MatrixXd robot_jacobian = calcErrorJacobian(joint_values); for (std::size_t i = 0; i < bounds_.size(); ++i) { - out.row(i) = constraint_derivative[i] * robot_jacobian.row(i); + out.row(i) = robot_jacobian.row(i); } } From 3404f1292e39e6fd1da520696a417de4111a67a8 Mon Sep 17 00:00:00 2001 From: BerendPijnenburg Date: Thu, 28 Mar 2024 13:48:58 +0100 Subject: [PATCH 2/4] Added unconstrained dimension functionality and consolidated BoxConstraint and EqualityConstraint --- .../ompl_interface/detail/ompl_constraints.h | 126 ++++---------- .../src/detail/ompl_constraints.cpp | 154 ++++++------------ 2 files changed, 89 insertions(+), 191 deletions(-) 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 fb47d60b4e..58bdda9a3a 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 @@ -86,7 +86,7 @@ class Bounds Eigen::VectorXd penalty(const Eigen::Ref& x) const; std::size_t size() const; - + private: std::vector lower_, upper_; std::size_t size_; @@ -248,101 +248,53 @@ class BaseConstraint : public ompl::base::Constraint * * These bounds are applied around the nominal position and orientation * of the box. - * */ -class BoxConstraint : public BaseConstraint -{ -public: - 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& 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& x) const override; -}; - -/****************************************** - * Equality Position Constraints - * ****************************************/ -/** \brief Equality constraints on a link's position. + * + * All constraints with a dimension lower than `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. + * + * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by + * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero + * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which + * the constraint is interpreted as an equality constraint. This threshold value is not used in the planning algorithm itself! * - * When you set the name of a constraint to 'use_equality_constraints', all constraints with a dimension lower than - * `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. + * This threshold value should be larger than the tolerance of the constraints specified in OMPL + * (ompl::magic::CONSTRAINT_PROJECTION_TOLERANCE = 1e-4). * - * The dimension value for the others are ignored. For example, a box with dimensions [1.0, 1e-5, 1.0] - * will result in equality constraints on the y-position, and no constraints on the x or z-position. + * This is necessary because the constraints are also checked by MoveIt in the StateValidity checker. If this check + * would use a stricter tolerance than was used to satisfy the constraints in OMPL, all states would be invalid. + * Therefore the dimension of an equality constraint specified in the constraint message should be at least equal the + * the tolerance used by OMPL to project onto the constraints. To avoid checking for exact equality on floats, the + * threshold is made a bit larger. * - * TODO(jeroendm) We could make this a base class `EqualityConstraints` with a specialization for position and orientation - * constraints in the future. But the direct overriding of `function` and `jacobian` is probably more performant. + * EQUALITY_CONSTRAINT_THRESHOLD > tolerance in constraint message > OMPL projection tolerance + * + * That's why the value is 1e-3 > 1e-4. + * Now the user can specify any value between 1e-3 and 1e-4 to specify an equality constraint. * */ -class EqualityPositionConstraint : public BaseConstraint +class BoxConstraint : public BaseConstraint { public: - 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; + BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs, const moveit_msgs::msg::PositionConstraint& pos_con); 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: - /** \brief Position bounds under this threshold are interpreted as equality constraints, the others as unbounded. - * - * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by - * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero - * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which - * the constraint is interpreted as an equality constraint. This threshold value is not used in the planning algorithm itself! - * - * This threshold value should be larger than the tolerance of the constraints specified in OMPL - * (ompl::magic::CONSTRAINT_PROJECTION_TOLERANCE = 1e-4). - * - * This is necessary because the constraints are also checked by MoveIt in the StateValidity checker. If this check - * would use a stricter tolerance than was used to satisfy the constraints in OMPL, all states would be invalid. - * Therefore the dimension of an equality constraint specified in the constraint message should be at least equal the - * the tolerance used by OMPL to project onto the constraints. To avoid checking for exact equality on floats, the - * threshold is made a bit larger. - * - * EQUALITY_CONSTRAINT_THRESHOLD > tolerance in constraint message > OMPL projection tolerance - * - * That's why the value is 1e-3 > 1e-4. - * Now the user can specify any value between 1e-3 and 1e-4 to specify an equality constraint. - * **/ - static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; + + std::vector getConstrainedDims(const moveit_msgs::msg::PositionConstraint& pos_con) const; - /** \brief Bool vector indicating which dimensions are constrained. **/ - std::vector is_dim_constrained_; +/** \brief + * + * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. + * The dimensions of the box are the bounds on the deviation of the link origin from + * the target pose, given in constraint_regions.primitive_poses[0]. + * */ + Bounds createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, const std::vector& constrained_dims) const; + + std::vector constrained_dims_; + + static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; }; /****************************************** @@ -410,14 +362,6 @@ class OrientationConstraint : public BaseConstraint Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; }; -/** \brief Extract position constraints from the MoveIt message. - * - * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. - * The dimensions of the box are the bounds on the deviation of the link origin from - * the target pose, given in constraint_regions.primitive_poses[0]. - * */ -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 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 0c59e4fb54..892c50df48 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -172,112 +172,91 @@ Eigen::MatrixXd BaseConstraint::calcErrorJacobian(const Eigen::Ref& x) const +void BoxConstraint::function(const Eigen::Ref& joint_values, Eigen::Ref out) const { - return target_orientation_.matrix().transpose() * (forwardKinematics(x).translation() - target_position_); -} + Eigen::Vector3d error = target_orientation_.matrix().transpose() * (forwardKinematics(joint_values).translation() - target_position_); -Eigen::MatrixXd BoxConstraint::calcErrorJacobian(const Eigen::Ref& x) const -{ - return target_orientation_.matrix().transpose() * robotGeometricJacobian(x).topRows(3); + int emplace_index = 0; + for (auto& dim : constrained_dims_) + { + out[emplace_index] = error[dim]; + emplace_index++; + } + out = bounds_.penalty(out); } -/****************************************** - * Equality constraints - * ****************************************/ -EqualityPositionConstraint::EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, - const std::string& group, const unsigned int num_dofs) - : BaseConstraint(robot_model, group, num_dofs) +void BoxConstraint::jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const { + Eigen::MatrixXd jac = target_orientation_.matrix().transpose() * robotGeometricJacobian(joint_values).topRows(3); + + int emplace_index = 0; + for (auto& dim : constrained_dims_) + { + out.row(emplace_index) = jac.row(dim); + emplace_index++; + } } -void EqualityPositionConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) +std::vector BoxConstraint::getConstrainedDims(const moveit_msgs::msg::PositionConstraint& pos_con) const { - const auto dims = constraints.position_constraints.at(0).constraint_region.primitives.at(0).dimensions; - - is_dim_constrained_ = { false, false, false }; + std::vector constrained_dims; + const auto dims = pos_con.constraint_region.primitives.at(0).dimensions; for (std::size_t i = 0; i < dims.size(); ++i) { - if (dims.at(i) < EQUALITY_CONSTRAINT_THRESHOLD) + if (dims.at(i) > 0 && dims.at(i) != std::numeric_limits::infinity()) { - if (dims.at(i) < getTolerance()) - { - RCLCPP_ERROR_STREAM( - getLogger(), - "Dimension: " << i - << " of position constraint is smaller than the tolerance used to evaluate the constraints. " - "This will make all states invalid and planning will fail. Please use a value between: " - << getTolerance() << " and " << EQUALITY_CONSTRAINT_THRESHOLD); - } - - is_dim_constrained_.at(i) = true; + constrained_dims.push_back(i); } } - - // extract target position and orientation - geometry_msgs::msg::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.position_constraints.at(0).constraint_region.primitive_poses.at(0).orientation, - target_orientation_); - - link_name_ = constraints.position_constraints.at(0).link_name; + return constrained_dims; } -void EqualityPositionConstraint::function(const Eigen::Ref& joint_values, - Eigen::Ref out) const +Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, const std::vector& constrained_dims) const { - Eigen::Vector3d error = - target_orientation_.matrix().transpose() * (forwardKinematics(joint_values).translation() - target_position_); - for (std::size_t dim = 0; dim < 3; ++dim) + std::vector lower; + std::vector upper; + + const auto dims = pos_con.constraint_region.primitives.at(0).dimensions; + + for (auto& i : constrained_dims) { - if (is_dim_constrained_.at(dim)) + if (dims.at(i) < getTolerance()) { - out[dim] = error[dim]; // equality constraint dimension + RCLCPP_ERROR_STREAM( + getLogger(), + "Dimension: " << i + << " of position constraint is smaller than the tolerance used to evaluate the constraints. " + "This will make all states invalid and planning will fail. Please use a value between: " + << getTolerance() << " and " << EQUALITY_CONSTRAINT_THRESHOLD); } - else + double value = 0; + if (dims.at(i) > EQUALITY_CONSTRAINT_THRESHOLD) { - out[dim] = 0.0; // unbounded dimension + value = dims.at(i) / 2; } + lower.push_back(-value); + upper.push_back(value); } -} -void EqualityPositionConstraint::jacobian(const Eigen::Ref& joint_values, - Eigen::Ref out) const -{ - out.setZero(); - Eigen::MatrixXd jac = target_orientation_.matrix().transpose() * robotGeometricJacobian(joint_values).topRows(3); - for (std::size_t dim = 0; dim < 3; ++dim) - { - if (is_dim_constrained_.at(dim)) - { - out.row(dim) = jac.row(dim); // equality constraint dimension - } - } + return { lower, upper}; } /****************************************** @@ -309,22 +288,6 @@ Eigen::MatrixXd OrientationConstraint::calcErrorJacobian(const Eigen::Ref::infinity(); - } - } - - return { { -dims.at(0) / 2.0, -dims.at(1) / 2.0, -dims.at(2) / 2.0 }, - { dims.at(0) / 2.0, dims.at(1) / 2.0, dims.at(2) / 2.0 } }; -} Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con) { @@ -372,16 +335,7 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo } else { - BaseConstraintPtr pos_con; - if (constraints.name == "use_equality_constraints") - { - pos_con = std::make_shared(robot_model, group, num_dofs); - } - else - { - pos_con = std::make_shared(robot_model, group, num_dofs); - } - pos_con->init(constraints); + BaseConstraintPtr pos_con = std::make_shared(robot_model, group, num_dofs, constraints.position_constraints.at(0)); ompl_constraints.emplace_back(pos_con); } } From 76c41022a028b5d81599a033d14754f73aa06781 Mon Sep 17 00:00:00 2001 From: BerendPijnenburg Date: Thu, 28 Mar 2024 13:57:11 +0100 Subject: [PATCH 3/4] Added unconstrained rotation dimension functionality. Removed redundant functions. --- .../ompl_interface/detail/ompl_constraints.h | 98 +++--------- .../src/detail/ompl_constraints.cpp | 139 ++++++++++-------- 2 files changed, 95 insertions(+), 142 deletions(-) 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); } From f180bd18ab7a6490d71d95c919225f60f4a8a6d3 Mon Sep 17 00:00:00 2001 From: BerendPijnenburg Date: Thu, 28 Mar 2024 17:59:53 +0100 Subject: [PATCH 4/4] Formatting --- .../ompl_interface/detail/ompl_constraints.h | 57 ++++++++-------- .../src/detail/ompl_constraints.cpp | 66 +++++++++++-------- 2 files changed, 68 insertions(+), 55 deletions(-) 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 06b8e07940..437fee75b7 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 @@ -124,14 +124,16 @@ class BaseConstraint : public ompl::base::Constraint * * OMPL requires you to override at least "function" which represents the constraint F(q) = 0 * */ - virtual void function(const Eigen::Ref& joint_values, Eigen::Ref out) const = 0; + 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. * * */ - virtual void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const = 0; + virtual void jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const = 0; /** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State. * @@ -209,9 +211,9 @@ class BaseConstraint : public ompl::base::Constraint * * These bounds are applied around the nominal position and orientation * of the box. - * + * * All constraints with a dimension lower than `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. - * + * * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which @@ -242,20 +244,20 @@ class BoxConstraint : public BaseConstraint void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; private: - std::vector getConstrainedDims(const moveit_msgs::msg::PositionConstraint& pos_con) const; -/** \brief - * - * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. - * The dimensions of the box are the bounds on the deviation of the link origin from - * the target pose, given in constraint_regions.primitive_poses[0]. - * */ - Bounds createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, const std::vector& constrained_dims) const; + /** \brief + * + * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. + * The dimensions of the box are the bounds on the deviation of the link origin from + * the target pose, given in constraint_regions.primitive_poses[0]. + * */ + Bounds createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, + const std::vector& constrained_dims) const; std::vector constrained_dims_; - static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; + static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; }; /****************************************** @@ -293,28 +295,25 @@ class OrientationConstraint : public BaseConstraint void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; private: - std::vector getConstrainedDims(const moveit_msgs::msg::OrientationConstraint& ori_con) const; -/** \brief - * - * 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 createBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con, const std::vector& constrained_dims) const; + /** \brief + * + * 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 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, const std::string& group, 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 3e0c0f44fc..afca769055 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -137,7 +137,7 @@ Eigen::MatrixXd BaseConstraint::robotGeometricJacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const +void BoxConstraint::function(const Eigen::Ref& joint_values, + Eigen::Ref out) const { - Eigen::Vector3d error = target_orientation_.matrix().transpose() * (forwardKinematics(joint_values).translation() - target_position_); + Eigen::Vector3d error = + target_orientation_.matrix().transpose() * (forwardKinematics(joint_values).translation() - target_position_); int emplace_index = 0; for (auto& dim : constrained_dims_) @@ -166,15 +168,16 @@ void BoxConstraint::function(const Eigen::Ref& joint_valu out = bounds_.penalty(out); } -void BoxConstraint::jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const +void BoxConstraint::jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const { Eigen::MatrixXd jac = target_orientation_.matrix().transpose() * robotGeometricJacobian(joint_values).topRows(3); int emplace_index = 0; for (auto& dim : constrained_dims_) { - out.row(emplace_index) = jac.row(dim); - emplace_index++; + out.row(emplace_index) = jac.row(dim); + emplace_index++; } } @@ -192,7 +195,8 @@ std::vector BoxConstraint::getConstrainedDims(const moveit_msgs::ms return constrained_dims; } -Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, const std::vector& constrained_dims) const +Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con, + const std::vector& constrained_dims) const { std::vector lower; std::vector upper; @@ -207,7 +211,7 @@ Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstrai getLogger(), "Dimension: " << i << " of position constraint is smaller than the tolerance used to evaluate the constraints. " - "This will make all states invalid and planning will fail. Please use a value between: " + "This will make all states invalid and planning will fail. Please use a value between: " << getTolerance() << " and " << EQUALITY_CONSTRAINT_THRESHOLD); } double value = 0; @@ -219,15 +223,16 @@ Bounds BoxConstraint::createBoundVector(const moveit_msgs::msg::PositionConstrai upper.push_back(value); } - return { lower, upper}; + return { lower, upper }; } /****************************************** * Orientation 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) +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) { constrained_dims_ = getConstrainedDims(ori_con); @@ -240,7 +245,8 @@ OrientationConstraint::OrientationConstraint(const moveit::core::RobotModelConst link_name_ = ori_con.link_name; } -void OrientationConstraint::function(const Eigen::Ref& joint_values, Eigen::Ref out) const +void OrientationConstraint::function(const Eigen::Ref& joint_values, + Eigen::Ref out) const { Eigen::Matrix3d orientation_difference = forwardKinematics(joint_values).linear().transpose() * target_orientation_; Eigen::AngleAxisd aa(orientation_difference); @@ -255,42 +261,49 @@ void OrientationConstraint::function(const Eigen::Ref& jo out = bounds_.penalty(out); } -void OrientationConstraint::jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const +void OrientationConstraint::jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const { Eigen::Matrix3d orientation_difference = forwardKinematics(joint_values).linear().transpose() * target_orientation_; Eigen::AngleAxisd aa{ orientation_difference }; - Eigen::MatrixXd jac = -angularVelocityToAngleAxis(aa.angle(), aa.axis()) * robotGeometricJacobian(joint_values).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++; + out.row(emplace_index) = jac.row(dim); + emplace_index++; } } -std::vector OrientationConstraint::getConstrainedDims(const moveit_msgs::msg::OrientationConstraint& ori_con) const +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 - if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_x_axis_tolerance != std::numeric_limits::infinity()) + 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()) + 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()) + 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 +Bounds OrientationConstraint::createBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con, + const std::vector& constrained_dims) const { std::vector lower; std::vector upper; @@ -312,7 +325,6 @@ Bounds OrientationConstraint::createBoundVector(const moveit_msgs::msg::Orientat lower.push_back(-ori_con.absolute_z_axis_tolerance); upper.push_back(ori_con.absolute_z_axis_tolerance); } - } return { lower, upper }; } @@ -349,7 +361,8 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo } else { - BaseConstraintPtr pos_con = std::make_shared(robot_model, group, num_dofs, constraints.position_constraints.at(0)); + BaseConstraintPtr pos_con = + std::make_shared(robot_model, group, num_dofs, constraints.position_constraints.at(0)); ompl_constraints.emplace_back(pos_con); } } @@ -362,7 +375,8 @@ 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, constraints.orientation_constraints.at(0)); + auto ori_con = std::make_shared(robot_model, group, num_dofs, + constraints.orientation_constraints.at(0)); ompl_constraints.emplace_back(ori_con); }