Skip to content

Commit

Permalink
Port OMPL orientation constraints to MoveIt2 (#1273)
Browse files Browse the repository at this point in the history 
* add orientation constraints to ompl interface

* also use constrained planner for a single orientation constraint

* Port orientation constraints to ROS2

* Update oreintation constraints for to use Bounds properly

* Fix warning text

* Use correct parameterization type

* Clang-tidy fixes and cleanup

* Apply suggestions from code review

Co-authored-by: AndyZe <andyz@utexas.edu>

* Removed all RCLCPP_DEBUG lines for consistency

* Add extra comments for doxygen, apply const &

* Fix logic for using the ConstrainedPlanningStateSpace

Co-authored-by: JeroenDM <jeroendemaeyer@live.be>
Co-authored-by: AndyZe <andyz@utexas.edu>
  • Loading branch information
@stephanie-eng @JeroenDM @AndyZe
3 people committed Jun 7, 2022
1 parent 4502659 commit 99c73e6
Show file tree
Hide file tree
Showing 4 changed files with 177 additions and 20 deletions.
2 changes: 1 addition & 1 deletion moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
View file
Expand Up @@ -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;
}
Expand Down
125 changes: 125 additions & 0 deletions moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h
View file
Expand Up @@ -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;
};

Expand All @@ -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;
Expand Down Expand Up @@ -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`.
Expand All @@ -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
63 changes: 46 additions & 17 deletions moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
View file
Expand Up @@ -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 =
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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
* **********************************/
Expand All @@ -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
* ****************************************/
Expand All @@ -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
{
Expand Down
7 changes: 5 additions & 2 deletions moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
View file
Expand Up @@ -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))
Expand Down

0 comments on commit 99c73e6

Please sign in to comment.