diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 5b0eb7ab..36471132 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -33,7 +34,8 @@ struct PandaMoveTo : public testing::Test scene = std::make_shared(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); - scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), + "extended"); t.add(std::make_unique("start", scene)); auto move = std::make_unique("move", std::make_shared()); @@ -50,7 +52,7 @@ struct PandaMoveTo : public testing::Test } TEST_F(PandaMoveTo, namedTarget) { - move_to->setGoal("extended"); + move_to->setGoal("ready"); EXPECT_ONE_SOLUTION; } @@ -70,29 +72,87 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } +geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { + state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); + auto frame_eigen{ state.getFrameTransform(frame) }; + geometry_msgs::PoseStamped p; + p.header.frame_id = state.getRobotModel()->getModelFrame(); + p.pose = tf2::toMsg(frame_eigen); + return p; +} + TEST_F(PandaMoveTo, pointTarget) { - move_to->setGoal([this]() { - RobotState state{ scene->getCurrentState() }; - state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); - auto frame{ state.getFrameTransform("panda_link8") }; - geometry_msgs::PointStamped point; - point.header.frame_id = scene->getPlanningFrame(); - point.point = tf2::toMsg(Eigen::Vector3d{ frame.translation() }); - return point; - }()); + geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") }; + + geometry_msgs::PointStamped point; + point.header = pose.header; + point.point = pose.pose.position; + move_to->setGoal(point); + EXPECT_ONE_SOLUTION; } TEST_F(PandaMoveTo, poseTarget) { - move_to->setGoal([this]() { - RobotState state{ scene->getCurrentState() }; - state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); - auto frame{ state.getFrameTransform("panda_link8") }; - geometry_msgs::PoseStamped pose; - pose.header.frame_id = scene->getPlanningFrame(); - pose.pose = tf2::toMsg(frame); - return pose; + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8")); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { + const std::string IK_FRAME{ "panda_hand" }; + move_to->setIKFrame(IK_FRAME); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + EXPECT_ONE_SOLUTION; +} + +moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, []() { + shape_msgs::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; }()); +#if MOVEIT_HAS_OBJECT_POSE + aco.object.pose.position.z = 0.2; + aco.object.pose.orientation.w = 1.0; +#else + aco.object.primitive_poses.resize(1); + aco.object.primitive_poses[0].position.z = 0.2; + aco.object.primitive_poses[0].orientation.w = 1.0; +#endif +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + aco.object.subframe_names.resize(1, "subframe"); + aco.object.subframe_poses.resize(1, []() { + geometry_msgs::Pose p; + p.orientation.w = 1.0; + return p; + }()); +#endif + return aco; +} + +TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + + move_to->setIKFrame(ATTACHED_OBJECT); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; + + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + + move_to->setIKFrame(IK_FRAME); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); EXPECT_ONE_SOLUTION; }