Skip to content

Commit

Permalink
Added unconstrained rotation dimension functionality. Removed redunda…
Browse files Browse the repository at this point in the history 
…nt functions.
  • Loading branch information
@BerendPijnenburg
BerendPijnenburg committed Mar 28, 2024
1 parent 3404f12 commit 76c4102
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 142 deletions.
98 changes: 19 additions & 79 deletions moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Jeroen De Maeyer, Boston Cleek */
/* Author: Jeroen De Maeyer, Boston Cleek, Berend Pijnenburg */

#pragma once

Expand Down Expand Up @@ -68,6 +68,7 @@ class Bounds
{
public:
Bounds();
Bounds(const std::vector<double>& lower, const std::vector<double>& upper);
/** \brief Distance to region inside bounds
*
* Distance of a given value outside the bounds, zero inside the bounds.
Expand Down Expand Up @@ -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<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> out) const override;
virtual void function(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> 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<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::MatrixXd> out) const override;
virtual void jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::MatrixXd> out) const = 0;

/** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State.
*
Expand All @@ -157,37 +149,6 @@ class BaseConstraint : public ompl::base::Constraint
* */
Eigen::MatrixXd robotGeometricJacobian(const Eigen::Ref<const Eigen::VectorXd>& 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<const Eigen::VectorXd>& /*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<const Eigen::VectorXd>& /*x*/) const;

// the methods below are specifically for debugging and testing

const std::string& getLinkName()
Expand Down Expand Up @@ -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<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> 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<const Eigen::VectorXd>& x) const override;
void jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::MatrixXd> 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<const Eigen::VectorXd>& x) const override;
};
private:

std::vector<std::size_t> 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.
Expand All @@ -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<std::size_t>& constrained_dims) const;

std::vector<std::size_t> 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,
Expand Down
139 changes: 76 additions & 63 deletions moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Jeroen De Maeyer, Boston Cleek */
/* Author: Jeroen De Maeyer, Boston Cleek, Berend Pijnenburg */

#include <algorithm>
#include <iterator>
Expand Down Expand Up @@ -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<const Eigen::VectorXd>& joint_values,
Eigen::Ref<Eigen::VectorXd> out) const
{
const Eigen::VectorXd current_values = calcError(joint_values);
out = bounds_.penalty(current_values);
}

void BaseConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
Eigen::Ref<Eigen::MatrixXd> 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<const Eigen::VectorXd>& joint_values) const
{
moveit::core::RobotState* robot_state = state_storage_.getStateStorage();
Expand All @@ -154,20 +131,6 @@ Eigen::MatrixXd BaseConstraint::robotGeometricJacobian(const Eigen::Ref<const Ei
return jacobian;
}

Eigen::VectorXd BaseConstraint::calcError(const Eigen::Ref<const Eigen::VectorXd>& /*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<const Eigen::VectorXd>& /*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
* ****************************************/
Expand Down Expand Up @@ -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<const Eigen::VectorXd>& x) const
void OrientationConstraint::function(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::VectorXd> 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<const Eigen::VectorXd>& x) const
void OrientationConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values, Eigen::Ref<Eigen::MatrixXd> 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<std::size_t> OrientationConstraint::getConstrainedDims(const moveit_msgs::msg::OrientationConstraint& ori_con) const
{
std::vector<std::size_t> 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<double>::infinity())
{
constrained_dims.push_back(0);
}
if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_y_axis_tolerance != std::numeric_limits<double>::infinity())
{
constrained_dims.push_back(1);
}
if (ori_con.absolute_x_axis_tolerance> 0 && ori_con.absolute_z_axis_tolerance != std::numeric_limits<double>::infinity())
{
constrained_dims.push_back(2);
}

return constrained_dims;
}

Bounds OrientationConstraint::createBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con, const std::vector<std::size_t>& constrained_dims) const
{
std::vector<double> 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<double> lower;
std::vector<double> 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<double>::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 };
}

/******************************************
Expand Down Expand Up @@ -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<OrientationConstraint>(robot_model, group, num_dofs);
ori_con->init(constraints);
auto ori_con = std::make_shared<OrientationConstraint>(robot_model, group, num_dofs, constraints.orientation_constraints.at(0));
ompl_constraints.emplace_back(ori_con);
}

Expand Down

0 comments on commit 76c4102

Please sign in to comment.