From ce9afe0130f8bd7143f1eb686ba538b6c8311161 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Mon, 28 Sep 2020 09:05:46 +0200 Subject: [PATCH 01/10] add parameterization type for orientation constraints and configure it from input message --- .../moveit/kinematic_constraints/kinematic_constraint.h | 7 +++++++ 1 file changed, 7 insertions(+) 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 51dfd6f763..28e3323b69 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 @@ -484,6 +484,13 @@ class OrientationConstraint : public KinematicConstraint } protected: + /** Describes the three parameter representation of an orientation matrix to apply constraint tolerance around x, y and x axis. **/ + enum class Parameterization + { + EULER_ANGLES, + ANGLE_AXIS + }; + 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. */ From 7bef2ed1f959bebf5cca3a811b0ddb2d27aaf186 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Sun, 1 Nov 2020 20:10:06 +0100 Subject: [PATCH 02/10] fiddle around with a test case for the new parameterization for orientation constriants --- .../test/test_orientation_constraints.cpp | 47 ++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index 9b9ddfdfff..6177a418de 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -72,7 +72,8 @@ class SphericalRobot : public testing::Test } protected: - moveit::core::RobotModelPtr robot_model_; + //moveit::core::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_wrist_model_; }; class FloatingJointRobot : public testing::Test @@ -474,6 +475,50 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } +// TEST_F(LoadPlanningModelsPr2, OrientationConstraintsParameterization) +// { +// moveit::core::RobotState robot_state(robot_model_); +// robot_state.setToDefaultValues(); +// robot_state.update(); +// moveit::core::Transforms tf(robot_model_->getModelFrame()); + +// kinematic_constraints::OrientationConstraint oc(robot_model_); + +// moveit_msgs::OrientationConstraint ocm; + +// // center the orientation constraints around the current orientation of the link +// geometry_msgs::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform("r_wrist_roll_link")); +// ocm.orientation = p.orientation; + +// ocm.link_name = "r_wrist_roll_link"; +// ocm.header.frame_id = robot_model_->getModelFrame(); +// // ocm.orientation.x = 0.0; +// // ocm.orientation.y = 0.0; +// // ocm.orientation.z = 0.0; +// // ocm.orientation.w = 1.0; +// ocm.absolute_x_axis_tolerance = 0.5; +// ocm.absolute_y_axis_tolerance = 0.5; +// ocm.absolute_z_axis_tolerance = 0.5; +// // ocm.parameterization = moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES; +// ocm.parameterization = 3; +// ocm.weight = 1.0; + +// EXPECT_TRUE(oc.configure(ocm, tf)); + +// // Constraints should be satisfied based on how we created them +// EXPECT_TRUE(oc.decide(robot_state).satisfied); + +// // move a joint and check the constraints again +// std::map jvals; +// jvals["r_wrist_flex_joint"] = 1.0; +// robot_state.setVariablePositions(jvals); +// robot_state.update(); + +// EXPECT_FALSE(oc.decide(robot_state, true).satisfied); + +// //oc.decide() +// } + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From b24e9db628847aa3ae38135e1cd3ce1f6dc11ea2 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Mon, 2 Nov 2020 20:14:36 +0100 Subject: [PATCH 03/10] orientation constraints: change class enum to int to correspond with moveit_msgs interface --- .../moveit/kinematic_constraints/kinematic_constraint.h | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 28e3323b69..c1f7768e97 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 @@ -483,14 +483,8 @@ class OrientationConstraint : public KinematicConstraint return parameterization_type_; } -protected: - /** Describes the three parameter representation of an orientation matrix to apply constraint tolerance around x, y and x axis. **/ - enum class Parameterization - { - EULER_ANGLES, - ANGLE_AXIS - }; +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. */ From f497ccb24b11354944e279dac79fe5f362b5efb9 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Mon, 2 Nov 2020 20:48:43 +0100 Subject: [PATCH 04/10] improve test for orientation constraints parameterization --- .../test/test_orientation_constraints.cpp | 70 +++++++------------ 1 file changed, 27 insertions(+), 43 deletions(-) diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index 6177a418de..ddec242640 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -46,6 +46,21 @@ class SphericalRobot : public testing::Test { protected: + /** A robot model with a single floating joint will be created. **/ + moveit::core::RobotModelPtr robot_model_; + + /** Test data + * + * The test data represents valid orientations for an absolute tolerance of 0.5 around x, y and z. + * + * The first matrix contains valid orientations for xyx_euler_angles error, but invalid for rotation_vector. + * The second matrix is the other way around. + * + * The rows contain a quaternion in the order x, y, z and w. + * */ + Eigen::Matrix valid_euler_data_; + Eigen::Matrix valid_rotvec_data_; + void SetUp() override { moveit::core::RobotModelBuilder builder("robot", "base_link"); @@ -326,6 +341,9 @@ TEST_F(SphericalRobot, Test4) TEST_F(SphericalRobot, Test5) { kinematic_constraints::OrientationConstraint oc(robot_model_); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_EQ(oc.getParameterization(), moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES); +} moveit::core::Transforms tf(robot_model_->getModelFrame()); moveit_msgs::OrientationConstraint ocm; @@ -475,49 +493,15 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } -// TEST_F(LoadPlanningModelsPr2, OrientationConstraintsParameterization) -// { -// moveit::core::RobotState robot_state(robot_model_); -// robot_state.setToDefaultValues(); -// robot_state.update(); -// moveit::core::Transforms tf(robot_model_->getModelFrame()); - -// kinematic_constraints::OrientationConstraint oc(robot_model_); - -// moveit_msgs::OrientationConstraint ocm; - -// // center the orientation constraints around the current orientation of the link -// geometry_msgs::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform("r_wrist_roll_link")); -// ocm.orientation = p.orientation; - -// ocm.link_name = "r_wrist_roll_link"; -// ocm.header.frame_id = robot_model_->getModelFrame(); -// // ocm.orientation.x = 0.0; -// // ocm.orientation.y = 0.0; -// // ocm.orientation.z = 0.0; -// // ocm.orientation.w = 1.0; -// ocm.absolute_x_axis_tolerance = 0.5; -// ocm.absolute_y_axis_tolerance = 0.5; -// ocm.absolute_z_axis_tolerance = 0.5; -// // ocm.parameterization = moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES; -// ocm.parameterization = 3; -// ocm.weight = 1.0; - -// EXPECT_TRUE(oc.configure(ocm, tf)); - -// // Constraints should be satisfied based on how we created them -// EXPECT_TRUE(oc.decide(robot_state).satisfied); - -// // move a joint and check the constraints again -// std::map jvals; -// jvals["r_wrist_flex_joint"] = 1.0; -// robot_state.setVariablePositions(jvals); -// robot_state.update(); - -// EXPECT_FALSE(oc.decide(robot_state, true).satisfied); - -// //oc.decide() -// } + ocm.parameterization = moveit_msgs::OrientationConstraint::ROTATION_VECTOR; + ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.3)); + EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); + EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied); + + ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.6)); + EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); + EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); +} int main(int argc, char** argv) { From 656bde655e44c61472dd37eb55424aeaf6867172 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Mon, 24 Aug 2020 17:38:27 +0200 Subject: [PATCH 05/10] add orientation constraints to ompl interface --- .../ompl_interface/detail/ompl_constraints.h | 70 +++++++++++++++++++ .../src/detail/ompl_constraints.cpp | 49 ++++++++++++- 2 files changed, 118 insertions(+), 1 deletion(-) 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 0c58968a79..21da665cb2 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 @@ -325,6 +325,44 @@ class EqualityPositionConstraint : public BaseConstraint std::vector is_dim_constrained_; }; +/****************************************** + * Orientation constraints + * ****************************************/ +/** \brief Orientation constraints parameterized using exponential coordinates. + * + * An orientation constraints is modelled 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 robot_model::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs) + : BaseConstraint(robot_model, group, num_dofs) + { + } + + void parseConstraintMsg(const moveit_msgs::Constraints& constraints) override; + Eigen::VectorXd calcError(const Eigen::Ref& x) const override; + virtual 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`. @@ -333,9 +371,41 @@ class EqualityPositionConstraint : public BaseConstraint * */ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::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. + * + * */ +std::vector orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con); + /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ std::shared_ptr createOMPLConstraint(const robot_model::RobotModelConstPtr& robot_model, const std::string& group, const moveit_msgs::Constraints& constraints); +/** Convert anglular velocity to angle-axis velocity, base on: + * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf + * */ +inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref axis) +{ + // Eigen::Matrix3d E; + + 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; + // return E; +} + } // namespace ompl_interface 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 e30d7c3754..7c8baa55a6 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -266,6 +266,37 @@ void EqualityPositionConstraint::jacobian(const Eigen::Ref& 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& 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 * **********************************/ @@ -283,6 +314,20 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::PositionConstraint& return { { -dims[0] / 2, -dims[1] / 2, -dims[2] / 2 }, { dims[0] / 2, dims[1] / 2, dims[2] / 2 } }; } +std::vector orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con) +{ + std::vector 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::infinity(); + } + return { { -dims[0], dims[0] }, { -dims[1], dims[1] }, { -dims[2], dims[2] } }; +} + /****************************************** * OMPL Constraints Factory * ****************************************/ @@ -332,7 +377,9 @@ std::shared_ptr createOMPLConstraint(const robot_model::RobotMod else if (num_ori_con > 0) { ROS_ERROR_NAMED(LOGNAME, "Orientation constraints are not yet supported."); - return nullptr; + auto ori_con = std::make_shared(robot_model, group, num_dofs); + ori_con->init(constraints); + return ori_con; } else { From a48044c0664ed73b849e36d3098f6977f661ab2d Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Thu, 27 Aug 2020 12:01:06 +0200 Subject: [PATCH 06/10] also use constrained planner for a single orientation constraint --- .../ompl/ompl_interface/src/planning_context_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index d42ea5b835..49a36a55f8 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -528,7 +528,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana if (constrained_planning_iterator != pc->second.config.end() && boost::lexical_cast(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_selector = std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE); From 87b6a3c961b58b2ebc10ba99e305d8e8f967ff4a Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Wed, 7 Apr 2021 19:52:18 +0200 Subject: [PATCH 07/10] correct orientation constraints parsing --- .../ompl_interface/detail/ompl_constraints.h | 9 ++--- .../src/detail/ompl_constraints.cpp | 34 ++++++++++++++----- 2 files changed, 28 insertions(+), 15 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 21da665cb2..cb8d26f997 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 @@ -353,14 +353,11 @@ class OrientationConstraint : public BaseConstraint { public: OrientationConstraint(const robot_model::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs) - : BaseConstraint(robot_model, group, num_dofs) - { - } + const unsigned int num_dofs); void parseConstraintMsg(const moveit_msgs::Constraints& constraints) override; Eigen::VectorXd calcError(const Eigen::Ref& x) const override; - virtual Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; + Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; }; /** \brief Extract position constraints from the MoveIt message. @@ -382,7 +379,7 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::PositionConstraint& * the width of the tolerance regions around the target orientation, represented using exponential coordinates. * * */ -std::vector orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con); +Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con); /** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ std::shared_ptr createOMPLConstraint(const robot_model::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 7c8baa55a6..afa3a652fb 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -269,20 +269,35 @@ void EqualityPositionConstraint::jacobian(const Eigen::Ref& x) const { Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_; @@ -314,7 +329,7 @@ Bounds positionConstraintMsgToBoundVector(const moveit_msgs::PositionConstraint& return { { -dims[0] / 2, -dims[1] / 2, -dims[2] / 2 }, { dims[0] / 2, dims[1] / 2, dims[2] / 2 } }; } -std::vector orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con) +Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConstraint& ori_con) { std::vector dims{ ori_con.absolute_x_axis_tolerance, ori_con.absolute_y_axis_tolerance, ori_con.absolute_z_axis_tolerance }; @@ -325,7 +340,8 @@ std::vector orientationConstraintMsgToBoundVector(const moveit_msgs::Ori if (dim == -1) dim = std::numeric_limits::infinity(); } - return { { -dims[0], dims[0] }, { -dims[1], dims[1] }, { -dims[2], dims[2] } }; + // return { { -dims[0], dims[0] }, { -dims[1], dims[1] }, { -dims[2], dims[2] } }; + return { { -dims[0], -dims[1], -dims[2] }, { dims[0] , dims[1] , dims[2] } }; } /****************************************** @@ -376,7 +392,7 @@ std::shared_ptr createOMPLConstraint(const robot_model::RobotMod } else if (num_ori_con > 0) { - ROS_ERROR_NAMED(LOGNAME, "Orientation constraints are not yet supported."); + ROS_ERROR_NAMED(LOGNAME, "OMPL is using orientation constraints."); auto ori_con = std::make_shared(robot_model, group, num_dofs); ori_con->init(constraints); return ori_con; From 7db63cd2a4a7956dc6d46e022043963c04971d7b Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Thu, 8 Apr 2021 17:29:48 +0200 Subject: [PATCH 08/10] debug merge --- .../src/detail/ompl_constraints.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) 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 afa3a652fb..cd7c88db2a 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -277,11 +277,11 @@ OrientationConstraint::OrientationConstraint(const robot_model::RobotModelConstP void OrientationConstraint::parseConstraintMsg(const moveit_msgs::Constraints& constraints) { - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsing Orientation constraints for OMPL constrained state space."); + ROS_INFO_STREAM_NAMED(LOGNAME, "Parsing Orientation constraints for OMPL constrained state space."); assert(bounds_.size() == 0); bounds_ = orientationConstraintMsgToBoundVector(constraints.orientation_constraints.at(0)); - ROS_DEBUG_NAMED(LOGNAME, "Parsed Orientation constraints"); - ROS_DEBUG_STREAM_NAMED(LOGNAME, bounds_); + ROS_INFO_NAMED(LOGNAME, "Parsed Orientation constraints"); + ROS_INFO_STREAM_NAMED(LOGNAME, bounds_); // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed rx / roll constraints" << bounds_[0]); // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed ry / pitch constraints" << bounds_[1]); // ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parsed rz / yaw constraints" << bounds_[2]); @@ -292,9 +292,14 @@ void OrientationConstraint::parseConstraintMsg(const moveit_msgs::Constraints& c // target_position_ << position.x, position.y, position.z; tf2::fromMsg(constraints.orientation_constraints.at(0).orientation, target_orientation_); + ROS_INFO_NAMED(LOGNAME, "Quaternion desired"); + ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.x()); + ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.y()); + ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.z()); + ROS_INFO_STREAM_NAMED(LOGNAME, target_orientation_.w()); link_name_ = constraints.orientation_constraints.at(0).link_name; - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Orientation constraints applied to link: " << link_name_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Orientation constraints applied to link: " << link_name_); } @@ -392,8 +397,9 @@ std::shared_ptr createOMPLConstraint(const robot_model::RobotMod } else if (num_ori_con > 0) { - ROS_ERROR_NAMED(LOGNAME, "OMPL is using orientation constraints."); - auto ori_con = std::make_shared(robot_model, group, num_dofs); + ROS_INFO_NAMED(LOGNAME, "OMPL is using orientation constraints."); + BaseConstraintPtr ori_con; + ori_con = std::make_shared(robot_model, group, num_dofs); ori_con->init(constraints); return ori_con; } From 48c75c254be9823363a0543cfc588ff6ea8c8011 Mon Sep 17 00:00:00 2001 From: Pradeep Rajendran Date: Fri, 2 Jul 2021 13:41:31 +0200 Subject: [PATCH 09/10] Removed code that sets default values to robot state and added unittests --- .../ompl/ompl_interface/CMakeLists.txt | 5 +- .../src/detail/threadsafe_state_storage.cpp | 1 - .../test/test_threadsafe_state_storage.cpp | 127 ++++++++++++++++++ 3 files changed, 131 insertions(+), 2 deletions(-) create mode 100644 moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 85b20ba5fb..eaf69bfea4 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -54,7 +54,6 @@ if(CATKIN_ENABLE_TESTING) # Unfortunately it is not possible to test the planning context manager without introducing a ROS dependency. find_package(rostest REQUIRED) - find_package(tf2_eigen REQUIRED) add_rostest_gtest(test_planning_context_manager test/test_planning_context_manager.test @@ -73,6 +72,10 @@ if(CATKIN_ENABLE_TESTING) target_link_libraries(test_constrained_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) set_target_properties(test_constrained_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + catkin_add_gtest(test_threadsafe_state_storage test/test_threadsafe_state_storage.cpp) + target_link_libraries(test_threadsafe_state_storage ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) + set_target_properties(test_threadsafe_state_storage PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + catkin_add_gtest(test_ompl_constraints test/test_ompl_constraints.cpp) target_link_libraries(test_ompl_constraints ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp index c40a40e590..e0cfa8245e 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp @@ -61,7 +61,6 @@ moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() cons if (it == thread_states_.end()) { st = new moveit::core::RobotState(start_state_); - st->setToDefaultValues(); // avoid uninitialized memory thread_states_[std::this_thread::get_id()] = st; } else diff --git a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp new file mode 100644 index 0000000000..8d89237b79 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp @@ -0,0 +1,127 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021 + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the authors nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Pradeep Rajendran */ +/* +This test checks if a state can be set in TSSafeStateStorage and correctly retrieved. +The skeleton of this test was taken from test_state_validity_checker.cpp by Jeroen De Maeyer. +*/ + +#include "load_test_robot.h" +#include +#include + +/** \brief This flag sets the verbosity level for the state validity checker. **/ +constexpr bool VERBOSE = false; + +constexpr char LOGNAME[] = "test_threadsafe_state_storage"; + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestThreadSafeStateStorage : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestThreadSafeStateStorage(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + /** This test if a state is correctly set in TSStateStorage. State is read back and compared with the original state **/ + void testReadback(const std::vector& position_in_limits) + { + SCOPED_TRACE("testConstruction"); + + // Set the robot_state_ to the given position + robot_state_->setJointGroupPositions(joint_model_group_, position_in_limits); + + // Construct the TSStateStorage using the constructor taking the robot state + ompl_interface::TSStateStorage const tss(*robot_state_); + + // Readback the stored state + auto robot_state_stored = tss.getStateStorage(); + + // Check if robot_state_stored's joint angles matches with what we set + for (auto const& joint_name : robot_state_->getVariableNames()) + { + auto const expected_value = robot_state_->getVariablePosition(joint_name); + auto const actual_value = robot_state_stored->getVariablePosition(joint_name); + EXPECT_EQ(actual_value, expected_value) << "Expecting joint value for " << joint_name << " to match."; + } + } + +protected: + void SetUp() override + { + } +}; + +// /*************************************************************************** +// * Run all tests on the Panda robot +// * ************************************************************************/ +class PandaTest : public TestThreadSafeStateStorage +{ +protected: + PandaTest() : TestThreadSafeStateStorage("panda", "panda_arm") + { + } +}; + +TEST_F(PandaTest, testConstruction) +{ + testReadback({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTest : public TestThreadSafeStateStorage +{ +protected: + FanucTest() : TestThreadSafeStateStorage("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTest, testConstructor) +{ + testReadback({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 7b81752bf3a0029d682132722f7f47cfdd34cf0d Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Fri, 2 Jul 2021 13:42:18 +0200 Subject: [PATCH 10/10] pre-commit --- .../kinematic_constraint.h | 1 - .../test/test_orientation_constraints.cpp | 48 +++++++++---------- .../src/detail/ompl_constraints.cpp | 26 +++++----- 3 files changed, 36 insertions(+), 39 deletions(-) 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 c1f7768e97..51dfd6f763 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 @@ -483,7 +483,6 @@ class OrientationConstraint : public KinematicConstraint return parameterization_type_; } - 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 diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index ddec242640..611489642c 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -87,7 +87,7 @@ class SphericalRobot : public testing::Test } protected: - //moveit::core::RobotModelPtr robot_model_; + // moveit::core::RobotModelPtr robot_model_; moveit::core::RobotModelPtr robot_wrist_model_; }; @@ -345,26 +345,26 @@ TEST_F(SphericalRobot, Test5) EXPECT_EQ(oc.getParameterization(), moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES); } - moveit::core::Transforms tf(robot_model_->getModelFrame()); - moveit_msgs::OrientationConstraint ocm; +moveit::core::Transforms tf(robot_model_->getModelFrame()); +moveit_msgs::OrientationConstraint ocm; - moveit::core::RobotState robot_state(robot_model_); - robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.0)); - robot_state.update(); +moveit::core::RobotState robot_state(robot_model_); +robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.0)); +robot_state.update(); - ocm.link_name = "yaw"; - ocm.header.frame_id = robot_model_->getModelFrame(); - ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear())); - ocm.absolute_x_axis_tolerance = 0.0; - ocm.absolute_y_axis_tolerance = 0.0; - ocm.absolute_z_axis_tolerance = 1.0; - ocm.weight = 1.0; +ocm.link_name = "yaw"; +ocm.header.frame_id = robot_model_->getModelFrame(); +ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear())); +ocm.absolute_x_axis_tolerance = 0.0; +ocm.absolute_y_axis_tolerance = 0.0; +ocm.absolute_z_axis_tolerance = 1.0; +ocm.weight = 1.0; - robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi(), 0.3)); - robot_state.update(); +robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi(), 0.3)); +robot_state.update(); - EXPECT_TRUE(oc.configure(ocm, tf)); - EXPECT_TRUE(oc.decide(robot_state, true).satisfied); +EXPECT_TRUE(oc.configure(ocm, tf)); +EXPECT_TRUE(oc.decide(robot_state, true).satisfied); } TEST_F(FloatingJointRobot, TestDefaultParameterization) @@ -493,14 +493,14 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } - ocm.parameterization = moveit_msgs::OrientationConstraint::ROTATION_VECTOR; - ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.3)); - EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); - EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied); +ocm.parameterization = moveit_msgs::OrientationConstraint::ROTATION_VECTOR; +ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.3)); +EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); +EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied); - ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.6)); - EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); - EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); +ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.6)); +EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); +EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } int main(int argc, char** argv) 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 cd7c88db2a..dafc264418 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -269,11 +269,11 @@ void EqualityPositionConstraint::jacobian(const Eigen::Ref& x) const { Eigen::Matrix3d orientation_difference = forwardKinematics(x).linear().transpose() * target_orientation_; @@ -346,7 +344,7 @@ Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::OrientationConst dim = std::numeric_limits::infinity(); } // return { { -dims[0], dims[0] }, { -dims[1], dims[1] }, { -dims[2], dims[2] } }; - return { { -dims[0], -dims[1], -dims[2] }, { dims[0] , dims[1] , dims[2] } }; + return { { -dims[0], -dims[1], -dims[2] }, { dims[0], dims[1], dims[2] } }; } /******************************************