diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index f5db2a69686..a1869ccc3d8 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -515,6 +515,9 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR) { Eigen::Vector3d rotation_vector(angle_x, angle_y, angle_z); + // convert rotation vector from frame_id to target frame + rotation_vector = + sampling_pose_.orientation_constraint_->getDesiredRotationMatrixInRefFrame().transpose() * rotation_vector; diff = Eigen::Isometry3d(Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized())); } else @@ -522,14 +525,10 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q /* The parameterization type should be validated in configure, so this should never happen. */ RCLCPP_ERROR(getLogger(), "The parameterization type for the orientation constraints is invalid."); } - // diff is isometry by construction - // getDesiredRotationMatrix() returns a valid rotation matrix by contract - // reqr has thus to be a valid isometry - Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); - quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized - // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the - // model + quat = Eigen::Quaterniond(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); + + // if this constraint is with respect to a mobile frame, we need to convert this rotation to the root frame of the model if (sampling_pose_.orientation_constraint_->mobileReferenceFrame()) { // getFrameTransform() returns a valid isometry by contract diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 0d5d2e96747..5684d55fcf5 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -336,12 +336,19 @@ MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPt /** * \brief Class for constraints on the orientation of a link * - * This class expresses an orientation constraint on a particular - * link. The constraint is specified in terms of a quaternion, with - * tolerances on X,Y, and Z axes. The rotation difference is computed - * based on the XYZ Euler angle formulation (intrinsic rotations) or as a rotation vector. This depends on the - * `Parameterization` type. The header on the quaternion can be specified in terms of either a fixed or a mobile - * frame. The type value will return ORIENTATION_CONSTRAINT. + * This class expresses an orientation constraint on a particular link. + * The constraint specifies a target orientation via a quaternion as well as + * tolerances on X,Y, and Z rotation axes. + * The rotation difference between the target and actual link orientation is expressed + * either as XYZ Euler angles or as a rotation vector (depending on the `parameterization` type). + * The latter is highly recommended, because it supports resolution of subframes and attached bodies. + * Also, rotation vector representation allows to interpret the tolerances always w.r.t. the given reference frame. + * Euler angles are much more restricted and exhibit singularities. + * + * For efficiency, if the target orientation is expressed w.r.t. to a fixed frame (relative to the planning frame), + * some stuff is precomputed. However, mobile reference frames are supported as well. + * + * The type value will return ORIENTATION_CONSTRAINT. * */ class OrientationConstraint : public KinematicConstraint @@ -439,6 +446,19 @@ class OrientationConstraint : public KinematicConstraint * * The returned matrix is always a valid rotation matrix. */ + const Eigen::Matrix3d& getDesiredRotationMatrixInRefFrame() const + { + // validity of the rotation matrix is enforced in configure() + return desired_R_in_frame_id_; + } + + /** + * \brief The rotation target in the reference or tf frame. + * + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. + */ const Eigen::Matrix3d& getDesiredRotationMatrix() const { // validity of the rotation matrix is enforced in configure() @@ -484,16 +504,15 @@ class OrientationConstraint : public KinematicConstraint } protected: - const moveit::core::LinkModel* link_model_; /**< \brief The target link model */ - Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame. Guaranteed to - * be valid rotation matrix. */ - Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for - * efficiency. Guaranteed to be valid rotation matrix. */ - std::string desired_rotation_frame_id_; /**< \brief The target frame of the transform tree */ - bool mobile_frame_; /**< \brief Whether or not the header frame is mobile or fixed */ - int parameterization_type_; /**< \brief Parameterization type for orientation tolerance. */ + const moveit::core::LinkModel* link_model_; /**< The target link model */ + Eigen::Matrix3d desired_R_in_frame_id_; /**< Desired rotation matrix in frame_id */ + Eigen::Matrix3d desired_rotation_matrix_; /**< The desired rotation matrix in the tf frame */ + Eigen::Matrix3d desired_rotation_matrix_inv_; /**< The inverse of desired_rotation_matrix_ (for efficiency) */ + std::string desired_rotation_frame_id_; /**< The target frame of the transform tree */ + bool mobile_frame_; /**< Whether or not the header frame is mobile or fixed */ + int parameterization_type_; /**< Parameterization type for orientation tolerance */ double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, - absolute_z_axis_tolerance_; /**< \brief Storage for the tolerances */ + absolute_z_axis_tolerance_; /**< Storage for the tolerances */ }; MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 33e4efbbde6..90dbcb5e791 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -597,7 +597,8 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra // clearing out any old data clear(); - link_model_ = robot_model_->getLinkModel(oc.link_name); + bool found; + link_model_ = robot_model_->getLinkModel(oc.link_name, &found); if (!link_model_) { RCLCPP_WARN(getLogger(), "Could not find link model for link name %s", oc.link_name.c_str()); @@ -617,6 +618,7 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra if (oc.header.frame_id.empty()) RCLCPP_WARN(getLogger(), "No frame specified for position constraint on link '%s'!", oc.link_name.c_str()); + desired_R_in_frame_id_ = Eigen::Quaterniond(q); // desired rotation wrt. frame_id if (tf.isFixedFrame(oc.header.frame_id)) { tf.transformQuaternion(oc.header.frame_id, q, q); @@ -749,10 +751,8 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob else if (parameterization_type_ == moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR) { Eigen::AngleAxisd aa(diff.linear()); - xyz_rotation = aa.axis() * aa.angle(); - xyz_rotation(0) = fabs(xyz_rotation(0)); - xyz_rotation(1) = fabs(xyz_rotation(1)); - xyz_rotation(2) = fabs(xyz_rotation(2)); + // transform rotation vector from target frame to frame_id and take absolute values + xyz_rotation = (desired_R_in_frame_id_ * (aa.axis() * aa.angle())).cwiseAbs(); } else { diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 6a66f6826f4..96fa5a70bca 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -217,6 +217,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n goal.orientation_constraints.resize(1); moveit_msgs::msg::OrientationConstraint& ocm = goal.orientation_constraints[0]; + ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR; ocm.link_name = link_name; ocm.header = pose.header; ocm.orientation = pose.pose.orientation; @@ -655,9 +656,17 @@ bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs: // the constraint needs to be expressed in the frame of a robot link. if (c.link_name != robot_link->getName()) { + if (c.parameterization == moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES) + { + RCLCPP_ERROR(getLogger(), + "Euler angles parameterization is not supported for non-link frames in orientation constraints. \n" + "Switch to rotation vector parameterization."); + return false; + } c.link_name = robot_link->getName(); Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() * state.getGlobalLinkTransform(robot_link).linear()); + // adapt goal orientation Eigen::Quaterniond quat_target; tf2::fromMsg(c.orientation, quat_target); c.orientation = tf2::toMsg(quat_target * link_name_to_robot_link); diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index ea0e49608d5..1a26e34abe3 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -475,6 +475,42 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } +TEST_F(FloatingJointRobot, ToleranceInRefFrame) +{ + moveit::core::RobotState robot_state(robot_model_); + robot_state.setToDefaultValues(); + auto base = rotationVectorToQuaternion(M_PI / 2.0, 0, 0); // base rotation: 90° about x + setRobotEndEffectorOrientation(robot_state, base); + robot_state.update(); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + + // create message to configure orientation constraints + moveit_msgs::msg::OrientationConstraint ocm; + ocm.link_name = "ee"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR; + ocm.orientation = tf2::toMsg(base); + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 0.2; + ocm.absolute_z_axis_tolerance = 1.0; + ocm.weight = 1.0; + + kinematic_constraints::OrientationConstraint oc(robot_model_); + EXPECT_TRUE(oc.configure(ocm, tf)); + + EXPECT_TRUE(oc.decide(robot_state).satisfied); // link and target are perfectly aligned + + // strong rotation w.r.t. base frame is ok + auto delta = rotationVectorToQuaternion(0.1, 0.1, 0.9); + setRobotEndEffectorOrientation(robot_state, delta * base); + EXPECT_TRUE(oc.decide(robot_state).satisfied); + + // strong rotation w.r.t. link frame is not ok + setRobotEndEffectorOrientation(robot_state, base * delta); + EXPECT_FALSE(oc.decide(robot_state).satisfied); // link and target are perfectly aligned +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);