From 9aefcb86288a87579b33f5816b24ffa9d09c3c34 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 6 Feb 2025 09:13:01 -0500 Subject: [PATCH] Reverts #2985, Ports moveit #3388 #3470 #3539 (backport #3284) (#3319) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Reverts #2985, Ports moveit #3388 #3470 #3539 (#3284) * 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 (cherry picked from commit 1794b8efa4c216be52272b8f6aa4af4e39838158) * Resolves merge conflicts. (#3323) --------- Co-authored-by: Mark Johnson <104826595+rr-mark@users.noreply.github.com> --- .../include/moveit/robot_model/robot_model.h | 6 +- moveit_core/robot_model/src/robot_model.cpp | 22 +- moveit_core/robot_state/CMakeLists.txt | 2 +- .../moveit/robot_state/attached_body.h | 7 - .../include/moveit/robot_state/robot_state.h | 10 +- .../src/cartesian_interpolator.cpp | 3 + 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, 398 insertions(+), 245 deletions(-) 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)