diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 7f5e387412..252e3dea39 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -255,6 +255,9 @@ class RobotModel LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr); /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints + * + * If jmg is given, all links that are not active in this JMG are considered fixed. + * Otherwise only fixed joints are considered fixed. * * This is useful, if the link should be warped to a specific pose using updateStateWithLinkAt(). * As updateStateWithLinkAt() warps only the specified link and its descendants, you might not @@ -265,7 +268,8 @@ class RobotModel * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...) * will actually warp wrist (and all its descendants). */ - static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link); + static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link, + const JointModelGroup* jmg = nullptr); /** \brief Get the array of links */ const std::vector& getLinkModels() const diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index fa22ee7389..9d1901826f 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1275,14 +1275,32 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) return nullptr; } -const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link) +const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link, const JointModelGroup* jmg) { if (!link) return link; + const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); const moveit::core::JointModel* joint = link->getParentJointModel(); + decltype(jmg->getJointModels().cbegin()) begin{}, end{}; + if (jmg) + { + begin = jmg->getJointModels().cbegin(); + end = jmg->getJointModels().cend(); + } + + // Returns whether `joint` is part of the rigidly connected chain. + // This is only false if the joint is both in `jmg` and not fixed. + auto is_fixed_or_not_in_jmg = [begin, end](const JointModel* joint) { + if (joint->getType() == JointModel::FIXED) + return true; + if (begin != end && // we do have a non-empty jmg + std::find(begin, end, joint) == end) // joint does not belong to jmg + return true; + return false; + }; - while (parent_link && joint->getType() == moveit::core::JointModel::FIXED) + while (parent_link && is_fixed_or_not_in_jmg(joint)) { link = parent_link; joint = link->getParentJointModel(); diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index fc318291bb..c9e75b077e 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -35,7 +35,7 @@ if(BUILD_TESTING) set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") endif() - ament_add_gtest(test_robot_state test/robot_state_test.cpp + ament_add_gmock(test_robot_state test/robot_state_test.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") target_link_libraries(test_robot_state diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 33aa9e535d..6df467b976 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -175,13 +175,6 @@ class AttachedBody * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; - /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) - * - * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const; - /** \brief Get the fixed transform to a named subframe on this body, relative to the world frame. * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 8bda2b6cc6..e5b0caa4ec 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1324,7 +1324,8 @@ class RobotState * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel, * but can additionally resolve parents for attached objects / subframes. */ - const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const; + const moveit::core::LinkModel* + getRigidlyConnectedParentLinkModel(const std::string& frame, const moveit::core::JointModelGroup* jmg = nullptr) const; /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. * This is typically the root link of the URDF unless a virtual joint is present. @@ -1893,6 +1894,13 @@ class RobotState /** \brief This function is only called in debug mode */ bool checkCollisionTransforms() const; + /** \brief Get the closest link in the kinematic tree to `frame`. + * + * Helper function for getRigidlyConnectedParentLinkModel, + * which resolves attached objects / subframes. + */ + const moveit::core::LinkModel* getLinkModelIncludingAttachedBodies(const std::string& frame) const; + RobotModelConstPtr robot_model_; void* memory_; diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 30b78df7fb..c7a5fc755e 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -161,7 +161,10 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // Random seeding (of additional attempts) would probably create IK jumps. if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options, cost_function)) + { + start_state->update(); traj.push_back(std::make_shared(*start_state)); + } else break; diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index b8e37950d7..29000846b8 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -799,14 +799,41 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]); } -const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const +const LinkModel* RobotState::getLinkModelIncludingAttachedBodies(const std::string& frame) const { - bool found; - const LinkModel* link{ nullptr }; - getFrameInfo(frame, link, found); - if (!found) - RCLCPP_ERROR(LOGGER, "Unable to find link for frame '%s'", frame.c_str()); - return getRobotModel()->getRigidlyConnectedParentLinkModel(link); + // If the frame is a link, return that link. + if (getRobotModel()->hasLinkModel(frame)) + { + return getLinkModel(frame); + } + + // If the frame is an attached body, return the link the body is attached to. + if (const auto it = attached_body_map_.find(frame); it != attached_body_map_.end()) + { + const auto& body{ it->second }; + return body->getAttachedLink(); + } + + // If the frame is a subframe of an attached body, return the link the body is attached to. + for (const auto& it : attached_body_map_) + { + const auto& body{ it.second }; + if (body->hasSubframeTransform(frame)) + { + return body->getAttachedLink(); + } + } + + // If the frame is none of the above, return nullptr. + return nullptr; +} + +const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame, + const moveit::core::JointModelGroup* jmg) const +{ + const LinkModel* link = getLinkModelIncludingAttachedBodies(frame); + + return getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg); } bool RobotState::satisfiesBounds(double margin) const diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index cdc49bbf62..df5b5604f4 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -193,179 +194,186 @@ class OneRobot : public testing::Test protected: void SetUp() override { - static const std::string MODEL2 = - "" - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - ""; - - static const std::string SMODEL2 = - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - ""; + static const std::string MODEL2 = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + static const std::string SMODEL2 = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); auto srdf_model = std::make_shared(); @@ -397,60 +405,25 @@ TEST_F(OneRobot, FK) ASSERT_TRUE(g_three != nullptr); ASSERT_TRUE(g_four == nullptr); - // joint_b is a fixed joint, so no one should have it - ASSERT_EQ(g_one->getJointModelNames().size(), 3u); - ASSERT_EQ(g_two->getJointModelNames().size(), 3u); - ASSERT_EQ(g_three->getJointModelNames().size(), 4u); - ASSERT_EQ(g_mim->getJointModelNames().size(), 2u); - - // only the links in between the joints, and the children of the leafs - ASSERT_EQ(g_one->getLinkModelNames().size(), 3u); - // g_two only has three links - ASSERT_EQ(g_two->getLinkModelNames().size(), 3u); - ASSERT_EQ(g_three->getLinkModelNames().size(), 4u); - - std::vector jmn = g_one->getJointModelNames(); - std::sort(jmn.begin(), jmn.end()); - EXPECT_EQ(jmn[0], "base_joint"); - EXPECT_EQ(jmn[1], "joint_a"); - EXPECT_EQ(jmn[2], "joint_c"); - jmn = g_two->getJointModelNames(); - std::sort(jmn.begin(), jmn.end()); - EXPECT_EQ(jmn[0], "base_joint"); - EXPECT_EQ(jmn[1], "joint_a"); - EXPECT_EQ(jmn[2], "joint_b"); - jmn = g_three->getJointModelNames(); - std::sort(jmn.begin(), jmn.end()); - EXPECT_EQ(jmn[0], "base_joint"); - EXPECT_EQ(jmn[1], "joint_a"); - EXPECT_EQ(jmn[2], "joint_b"); - EXPECT_EQ(jmn[3], "joint_c"); + EXPECT_THAT(g_one->getJointModelNames(), ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_c" })); + EXPECT_THAT(g_two->getJointModelNames(), ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_b" })); + EXPECT_THAT(g_three->getJointModelNames(), + ::testing::ElementsAreArray({ "base_joint", "joint_a", "joint_b", "joint_c" })); + EXPECT_THAT(g_mim->getJointModelNames(), ::testing::ElementsAreArray({ "mim_f", "joint_f" })); - // but they should have the same links to be updated - ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6u); - ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6u); - ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6u); - - EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(), "base_link"); - EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(), "link_a"); - EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(), "link_b"); - EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(), "link_c"); - - EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(), "base_link"); - EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(), "link_a"); - EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(), "link_b"); - EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(), "link_c"); - - EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(), "base_link"); - EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(), "link_a"); - EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(), "link_b"); - EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(), "link_c"); + EXPECT_THAT(g_one->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_c" })); + EXPECT_THAT(g_two->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_b" })); + EXPECT_THAT(g_three->getLinkModelNames(), ::testing::ElementsAreArray({ "base_link", "link_a", "link_b", "link_c" })); - // bracketing so the state gets destroyed before we bring down the model + // but they should have the same links to be updated + auto updated_link_model_names = { "base_link", "link_a", "link_b", "link_c", "link_d", "link_e", "link/with/slash" }; + EXPECT_THAT(g_one->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names)); + EXPECT_THAT(g_two->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names)); + EXPECT_THAT(g_three->getUpdatedLinkModelNames(), ::testing::ElementsAreArray(updated_link_model_names)); moveit::core::RobotState state(model); - EXPECT_EQ((unsigned int)7, state.getVariableCount()); + EXPECT_EQ(state.getVariableCount(), 7u); state.setToDefaultValues(); @@ -692,17 +665,18 @@ TEST_F(OneRobot, rigidlyConnectedParent) moveit::core::RobotState state(robot_model_); state.setToDefaultValues(); - state.updateLinkTransforms(); + Eigen::Isometry3d a_to_b; EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); // attach "object" with "subframe" to link_b state.attachBody(std::make_unique( - link_b, "object", Eigen::Isometry3d::Identity(), std::vector{}, + link_b, "object", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)), std::vector{}, EigenSTL::vector_Isometry3d{}, std::set{}, trajectory_msgs::msg::JointTrajectory{}, - moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } })); + moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } })); // RobotState's version should resolve these too + Eigen::Isometry3d transform; EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object")); EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe")); @@ -712,6 +686,25 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("")); EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/")); EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/")); + + // link names with '/' should still work as before + const moveit::core::LinkModel* link_with_slash{ robot_model_->getLinkModel("link/with/slash") }; + EXPECT_TRUE(link_with_slash); + const moveit::core::LinkModel* rigid_parent_of_link_with_slash = + state.getRigidlyConnectedParentLinkModel("link/with/slash"); + ASSERT_TRUE(rigid_parent_of_link_with_slash); + EXPECT_EQ("base_link", rigid_parent_of_link_with_slash->getName()); + + // the last /-separated component of an object might be a subframe + state.attachBody(std::make_unique( + link_with_slash, "object/with/slash", Eigen::Isometry3d(Eigen::Translation3d(1, 0, 0)), + std::vector{}, EigenSTL::vector_Isometry3d{}, std::set{}, + trajectory_msgs::msg::JointTrajectory{}, + moveit::core::FixedTransformsMap{ { "sub/frame", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)) } })); + const moveit::core::LinkModel* rigid_parent_of_object = + state.getRigidlyConnectedParentLinkModel("object/with/slash/sub/frame"); + ASSERT_TRUE(rigid_parent_of_object); + EXPECT_EQ(rigid_parent_of_link_with_slash, rigid_parent_of_object); } int main(int argc, char** argv) diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 429dd9d802..ef8b7aef48 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -345,6 +345,18 @@ TEST_F(MoveGroupTestFixture, JointSpaceGoalTest) testJointPositions(plan_joint_positions); } +TEST_F(MoveGroupTestFixture, CartesianGoalTest) +{ + move_group_->setPoseReferenceFrame("world"); + move_group_->setEndEffectorLink("panda_hand"); + geometry_msgs::Pose pose; + pose.position.x = 0.417; + pose.position.y = 0.240; + pose.position.z = 0.532; + pose.orientation.w = 1.0; + EXPECT_TRUE(move_group_->setJointValueTarget(pose)); +} + int main(int argc, char** argv) { ros::init(argc, argv, "move_group_interface_cpp_test"); diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index f87eab588d..7cc55a3cc2 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -84,6 +84,43 @@ bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_int return false; } +// similar to MoveToCartPose, but tries to plan a cartesian path with a subframe link +bool moveCartesianPath(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, + const std::string& end_effector_link) +{ + group.clearPoseTargets(); + group.setEndEffectorLink(end_effector_link); + group.setStartStateToCurrentState(); + std::vector initial_joint_position({ 0, -TAU / 8, 0, -3 * TAU / 8, 0, TAU / 4, TAU / 8 }); + group.setJointValueTarget(initial_joint_position); + moveit::planning_interface::MoveGroupInterface::Plan myplan; + if (!group.plan(myplan) || !group.execute(myplan)) + { + ROS_WARN("Failed to move to initial joint positions"); + return false; + } + + std::vector waypoints; + waypoints.push_back(pose.pose); + moveit_msgs::RobotTrajectory trajectory; + double percent = group.computeCartesianPath(waypoints, 0.01, 0, trajectory, true); + if (percent == 1.0) + { + group.execute(trajectory); + return true; + } + + if (percent == -1.0) + { + ROS_WARN("Failed to compute cartesian path"); + } + else + { + ROS_WARN_STREAM("Computed only " << percent * 100.0 << "% of path"); + } + return false; +} + // Function copied from subframes tutorial // This helper function creates two objects and publishes them to the PlanningScene: a box and a cylinder. // The box spawns in front of the gripper, the cylinder at the tip of the gripper, as if it had been grasped. @@ -169,7 +206,7 @@ TEST(TestPlanUsingSubframes, SubframesTests) target_pose_stamped.pose.orientation = tf2::toMsg(target_orientation); target_pose_stamped.pose.position.z = Z_OFFSET; - ROS_INFO_STREAM_NAMED(log_name, "Moving to bottom of box with cylinder tip"); + ROS_INFO_STREAM_NAMED(log_name, "Moving to bottom of box with cylinder tip, and then away"); target_pose_stamped.header.frame_id = "box/bottom"; ASSERT_TRUE(moveToCartPose(target_pose_stamped, group, "cylinder/tip")); planning_scene_monitor->requestPlanningSceneState(); @@ -197,6 +234,64 @@ TEST(TestPlanUsingSubframes, SubframesTests) ss << "panda link frame: \n" << panda_link.matrix() << "\nexpected pose: \n" << expected_pose.matrix(); EXPECT_TRUE(panda_link.isApprox(expected_pose, EPSILON)) << ss.str(); } + att_coll_object.object.operation = att_coll_object.object.REMOVE; + planning_scene_interface.applyAttachedCollisionObject(att_coll_object); + moveit_msgs::CollisionObject coll_object1, coll_object2; + coll_object1.id = "cylinder"; + coll_object1.operation = moveit_msgs::CollisionObject::REMOVE; + coll_object2.id = "box"; + coll_object2.operation = moveit_msgs::CollisionObject::REMOVE; + planning_scene_interface.applyCollisionObject(coll_object1); + planning_scene_interface.applyCollisionObject(coll_object2); +} + +TEST(TestPlanUsingSubframes, SubframesCartesianPathTests) +{ + const std::string log_name = "test_cartesian_path_using_subframes"; + ros::NodeHandle nh(log_name); + + auto planning_scene_monitor = std::make_shared("robot_description"); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + moveit::planning_interface::MoveGroupInterface group("panda_arm"); + group.setPlanningTime(PLANNING_TIME_S); + + spawnCollisionObjects(planning_scene_interface); + moveit_msgs::CollisionObject coll_object2; + coll_object2.id = "box"; + coll_object2.operation = moveit_msgs::CollisionObject::REMOVE; + planning_scene_interface.applyCollisionObject(coll_object2); + + moveit_msgs::AttachedCollisionObject att_coll_object; + att_coll_object.object.id = "cylinder"; + att_coll_object.link_name = "panda_hand"; + att_coll_object.object.operation = att_coll_object.object.ADD; + att_coll_object.object.pose.orientation.w = 1.0; + planning_scene_interface.applyAttachedCollisionObject(att_coll_object); + + // Move to where panda_hand is at when it graps cylinder + geometry_msgs::PoseStamped target_pose_stamped; + target_pose_stamped = group.getCurrentPose("panda_hand"); + tf2::Quaternion orientation; + orientation.setRPY(TAU / 2, -TAU / 4.0, 0); + target_pose_stamped.pose.orientation = tf2::toMsg(orientation); + + ROS_INFO_STREAM_NAMED(log_name, "Moving hand in cartesian path to hand grasping location"); + ASSERT_TRUE(moveCartesianPath(target_pose_stamped, group, "cylinder/tip")); + planning_scene_monitor->requestPlanningSceneState(); + { + planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); + + // get the tip and base frames + Eigen::Isometry3d cyl_tip = planning_scene->getFrameTransform("cylinder/tip"); + Eigen::Isometry3d base = planning_scene->getFrameTransform(target_pose_stamped.header.frame_id); + Eigen::Isometry3d target_pose; + tf2::fromMsg(target_pose_stamped.pose, target_pose); + + // expect that they are identical + std::stringstream ss; + ss << "target frame: \n" << (base * target_pose).matrix() << "\ncylinder frame: \n" << cyl_tip.matrix(); + EXPECT_TRUE(cyl_tip.isApprox(base * target_pose, EPSILON)) << ss.str(); + } } int main(int argc, char** argv)