From e1b1635c23f42e89efd7bd6670ff4df302aa814d Mon Sep 17 00:00:00 2001 From: Mark Johnson <104826595+rr-mark@users.noreply.github.com> Date: Thu, 6 Feb 2025 02:27:17 +0000 Subject: [PATCH] Reverts #2985, Ports moveit #3388 #3470 #3539 (#3284) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)" This reverts commit 1f23344de0293c76cba75361300e1a6b098e8d5a. * Merge PR #3388: Generalize RobotState::setFromIK() So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame. This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an active joint from the arm's tooltip. However, as long as this joint is not part of the JMG, the corresponding transform can be considered as fixed as well. This PR generalizes the functions getRigidlyConnectedParentLinkModel() in RobotState and RobotModel to receive an optional JMG pointer. If present, only (active) joints from that group are considered non-fixed. This PR also enables subframe support for setFromIK - simply by using getRigidlyConnectedParentLinkModel(), which already supported that. There is one drawback of this approach: A repeated application of setFromIK with the same target frame and JMG (as in computeCartesianPath()), will repeat the search for the common fixed parent link. Additionally, the passed RobotState needs to be up-to-date. We could mitigate this by pulling the corresponding code into a separate function and calling it once in computeCartesianPath(). * Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel() Fixes #3388 * Merge pull request #3539 from v4hn/find-links-with-slashes-again find links with slashes again * Ports to ROS2 and fixes problems introduced in merge conflicts. * Fixes formatting. * Makes robot_state_test.cpp include gmock. * Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory. * Adds braces to make clang-tidy happy. * Removes test-only arguments; adds more comments. * Fixes formatting. * Fixes formatting. * Adds missing class scope. --------- Co-authored-by: Robert Haschke Co-authored-by: Robert Haschke Co-authored-by: Michael Görner Co-authored-by: Sebastian Jahr (cherry picked from commit 1794b8efa4c216be52272b8f6aa4af4e39838158) --- .../moveit/robot_model/robot_model.hpp | 6 +- moveit_core/robot_model/src/robot_model.cpp | 22 +- moveit_core/robot_state/CMakeLists.txt | 2 +- .../moveit/robot_state/attached_body.hpp | 7 - .../moveit/robot_state/robot_state.hpp | 10 +- .../src/cartesian_interpolator.cpp | 1 + moveit_core/robot_state/src/robot_state.cpp | 41 +- .../robot_state/test/robot_state_test.cpp | 443 +++++++++--------- .../test/move_group_interface_cpp_test.cpp | 12 + .../test/subframes_test.cpp | 97 +++- 10 files changed, 395 insertions(+), 246 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp index 2cc06f984d..343324bd18 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp @@ -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 c6634f5b5d..caaf8d197e 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1364,14 +1364,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 840e6ba4e7..a906be7069 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -37,7 +37,7 @@ if(BUILD_TESTING) "${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 moveit_test_utils moveit_utils diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp b/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp index 4ef09a2518..30e95c4ea8 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp @@ -167,13 +167,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.hpp b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp index 7f2e3637f8..599071f374 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp @@ -1231,7 +1231,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. @@ -1712,6 +1713,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_; std::vector position_; diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 0c85e910e9..f657da5983 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -455,6 +455,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options, cost_function)) { + start_state->update(); path.push_back(std::make_shared(*start_state)); } else diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index d24a19b371..a3fded62bf 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -907,14 +907,41 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome } } -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(getLogger(), "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); } const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint) diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 5af16e3788..52b86d4c9f 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 @@ -247,180 +248,185 @@ class OneRobot : public testing::Test void SetUp() override { static const std::string MODEL2 = R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - )"; - - static const std::string SMODEL2 = R"xml( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - )xml"; + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + static const std::string SMODEL2 = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); auto srdf_model = std::make_shared(); @@ -480,60 +486,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"); - - // 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_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" })); - 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(7u, state.getVariableCount()); + EXPECT_EQ(state.getVariableCount(), 7u); state.setToDefaultValues(); @@ -782,17 +753,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")); @@ -802,6 +774,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); } TEST(getJacobian, RevoluteJoints) 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 13b98f75aa..e7bc92da72 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 @@ -348,6 +348,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 8cafabfb77..07ccc0817c 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)