diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index 9b9ddfdfff..611489642c 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"); @@ -72,7 +87,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 @@ -325,27 +341,30 @@ 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; +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) @@ -474,6 +493,16 @@ 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.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) { testing::InitGoogleTest(&argc, argv); 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/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h index 0c58968a79..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 @@ -325,6 +325,41 @@ 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); + + void parseConstraintMsg(const moveit_msgs::Constraints& constraints) override; + Eigen::VectorXd calcError(const Eigen::Ref& x) const override; + 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 +368,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. + * + * */ +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, 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..dafc264418 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,55 @@ 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 +332,21 @@ 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 } }; } +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 }; + + // 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] } }; + return { { -dims[0], -dims[1], -dims[2] }, { dims[0], dims[1], dims[2] } }; +} + /****************************************** * OMPL Constraints Factory * ****************************************/ @@ -331,8 +395,11 @@ 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; + 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; } else { 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/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); 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(); +}