From f180bd18ab7a6490d71d95c919225f60f4a8a6d3 Mon Sep 17 00:00:00 2001 From: BerendPijnenburg Date: Thu, 28 Mar 2024 17:59:53 +0100 Subject: [PATCH] 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); }