From e60a9793f10861ec07d6772f73f54fb1731aa350 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 Oct 2021 09:48:10 +0200 Subject: [PATCH 01/12] fix test helper never unload the plugin loader before the plugins (IK plugins here). We don't have unrelated loaders in gtest executables, so the static should be fine. --- core/test/models.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/test/models.cpp b/core/test/models.cpp index 9d514b1c..377df953 100644 --- a/core/test/models.cpp +++ b/core/test/models.cpp @@ -18,6 +18,6 @@ RobotModelPtr getModel() { } moveit::core::RobotModelPtr loadModel() { - robot_model_loader::RobotModelLoader loader; + static robot_model_loader::RobotModelLoader loader; return loader.getModel(); } From dfe746b9bb56b0044618cb06db242ac6ca4f82e4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 Oct 2021 09:49:47 +0200 Subject: [PATCH 02/12] FixedState: add optional scene in constructor --- core/include/moveit/task_constructor/stages/fixed_state.h | 2 +- core/src/stages/fixed_state.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 59a434cb..2ace2726 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -48,7 +48,7 @@ namespace stages { class FixedState : public Generator { public: - FixedState(const std::string& name = "initial state"); + FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr); void setState(const planning_scene::PlanningScenePtr& scene); void reset() override; diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index fddc9e4d..71ad60ad 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -43,7 +43,8 @@ namespace moveit { namespace task_constructor { namespace stages { -FixedState::FixedState(const std::string& name) : Generator(name) { +FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) + : Generator(name), scene_(scene) { setCostTerm(std::make_unique(0.0)); } From ef86799f272eef8b0bcf32051bd22d999e56059d Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 Oct 2021 09:51:45 +0200 Subject: [PATCH 03/12] add some tests for MoveTo --- core/test/CMakeLists.txt | 3 ++ core/test/move_to.test | 7 +++ core/test/test_move_to.cpp | 106 +++++++++++++++++++++++++++++++++++++ 3 files changed, 116 insertions(+) create mode 100644 core/test/move_to.test create mode 100644 core/test/test_move_to.cpp diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 4ccd5278..0b530ed1 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -33,6 +33,9 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) + add_rostest_gtest(${PROJECT_NAME}-test-move-to move_to.test test_move_to.cpp) + target_link_libraries(${PROJECT_NAME}-test-move-to ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) + # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) diff --git a/core/test/move_to.test b/core/test/move_to.test new file mode 100644 index 00000000..105e936a --- /dev/null +++ b/core/test/move_to.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp new file mode 100644 index 00000000..8eacfdb0 --- /dev/null +++ b/core/test/test_move_to.cpp @@ -0,0 +1,106 @@ +#include "models.h" + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveTo : public testing::Test +{ + Task t; + stages::MoveTo* move_to; + PlanningScenePtr scene; + + PandaMoveTo() { + t.setRobotModel(loadModel()); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move = std::make_unique("move", std::make_shared()); + move_to = move.get(); + move_to->setGroup("panda_arm"); + t.add(std::move(move)); + } +}; + +#define EXPECT_ONE_SOLUTION \ + { \ + EXPECT_TRUE(t.plan()); \ + EXPECT_EQ(t.solutions().size(), 1u); \ + } + +TEST_F(PandaMoveTo, namedTarget) { + move_to->setGoal("extended"); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, mapTarget) { + move_to->setGoal(std::map{ { "panda_joint1", TAU / 8 }, { "panda_joint2", TAU / 8 } }); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, stateTarget) { + move_to->setGoal([]() { + moveit_msgs::RobotState state; + state.is_diff = true; + state.joint_state.name = { "panda_joint1", "panda_joint2" }; + state.joint_state.position = { TAU / 8, TAU / 8 }; + return state; + }()); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, DISABLED_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; + }()); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, DISABLED_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; + }()); + EXPECT_ONE_SOLUTION; +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "move_to_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +} From 86fe752d43e0aa1d6eb5481ac6cb135ccd77b4e8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 Oct 2021 09:52:51 +0200 Subject: [PATCH 04/12] InterpolationPlanner: implement simple IK-based solver for pose targets --- core/src/solvers/joint_interpolation.cpp | 44 +++++++++++++++++++----- core/test/test_move_to.cpp | 4 +-- 2 files changed, 38 insertions(+), 10 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index a0aa2202..35b3935f 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -40,6 +40,8 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace solvers { @@ -53,9 +55,9 @@ void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_mod bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::Constraints& /*path_constraints*/) { const auto& props = properties(); // Get maximum joint distance @@ -95,12 +97,38 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return true; } -bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/, - const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/, - const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/, - robot_trajectory::RobotTrajectoryPtr& /*result*/, - const moveit_msgs::Constraints& /*path_constraints*/) { - throw std::runtime_error("Not yet implemented"); +bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + const auto start_time = std::chrono::steady_clock::now(); + + auto to{ from->diff() }; + + kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() }; + constraints.add(path_constraints, from->getTransforms()); + + auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg, + const double* joint_values) -> bool { + robot_state->setJointGroupPositions(jmg, joint_values); + robot_state->update(); + return to->isStateValid(*robot_state, constraints, jmg->getName()); + } }; + + if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + // TODO(v4hn): planners need a way to add feedback to failing plans + // in case of an invalid solution feedback should include unwanted collisions or violated constraints + ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); + return false; + } + to->getCurrentStateNonConst().update(); + + timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); + if (timeout <= 0.0) + return false; + + return plan(from, to, jmg, timeout, result, path_constraints); } } // namespace solvers } // namespace task_constructor diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 8eacfdb0..5b0eb7ab 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -70,7 +70,7 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } -TEST_F(PandaMoveTo, DISABLED_pointTarget) { +TEST_F(PandaMoveTo, pointTarget) { move_to->setGoal([this]() { RobotState state{ scene->getCurrentState() }; state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); @@ -83,7 +83,7 @@ TEST_F(PandaMoveTo, DISABLED_pointTarget) { EXPECT_ONE_SOLUTION; } -TEST_F(PandaMoveTo, DISABLED_poseTarget) { +TEST_F(PandaMoveTo, poseTarget) { move_to->setGoal([this]() { RobotState state{ scene->getCurrentState() }; state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended"); From aee76fee5e2c1765036a3a97bff299688b4867e5 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 Oct 2021 21:12:14 +0200 Subject: [PATCH 05/12] add move MoveTo tests (partially disabled because broken) --- core/test/test_move_to.cpp | 98 ++++++++++++++++++++++++++++++-------- 1 file changed, 79 insertions(+), 19 deletions(-) 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; } From e1216aa8ab3214f8ef5aada37bbea38b5be41c62 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 Oct 2021 15:52:15 +0200 Subject: [PATCH 06/12] MoveTo supports attached objects&subframes for ik frame --- .../moveit/task_constructor/moveit_compat.h | 17 ++++ .../moveit/task_constructor/stages/move_to.h | 2 +- core/src/stages/move_to.cpp | 88 +++++++++++++------ core/test/test_move_to.cpp | 8 +- 4 files changed, 84 insertions(+), 31 deletions(-) diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 5024861f..bf67ac07 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -55,3 +55,20 @@ // use object shape poses relative to a single object pose #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) + +#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) + +#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK +#include +inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, + std::string frame) { + const moveit::core::LinkModel* link{ nullptr }; + + if (state.hasAttachedBody(frame)) { + link = state.getAttachedBody(frame)->getAttachedLink(); + } else if (state.getRobotModel()->hasLinkModel(frame)) + link = state.getLinkModel(frame); + + return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); +} +#endif diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 98bf0f57..5968017b 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -99,7 +99,7 @@ protected: bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state); bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen); - bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, + bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen); protected: diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f44bd702..f24a908d 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -42,6 +42,8 @@ #include #include +#include + #include namespace moveit { @@ -142,33 +144,29 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint } bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, - Eigen::Isometry3d& target_eigen) { + Eigen::Isometry3d& target) { try { - const geometry_msgs::PoseStamped& target = boost::any_cast(goal); - tf2::fromMsg(target.pose, target_eigen); - + const geometry_msgs::PoseStamped& msg = boost::any_cast(goal); + tf2::fromMsg(msg.pose, target); // transform target into global frame - const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); - target_eigen = frame * target_eigen; + target = scene->getFrameTransform(msg.header.frame_id) * target; } catch (const boost::bad_any_cast&) { return false; } return true; } -bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, +bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) { try { const geometry_msgs::PointStamped& target = boost::any_cast(goal); Eigen::Vector3d target_point; tf2::fromMsg(target.point, target_point); - // transform target into global frame - const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); - target_point = frame * target_point; + target_point = scene->getFrameTransform(target.header.frame_id) * target_point; // retain link orientation - target_eigen = scene->getCurrentState().getGlobalLinkTransform(link); + target_eigen = ik_pose; target_eigen.translation() = target_point; } catch (const boost::bad_any_cast&) { return false; @@ -204,47 +202,81 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { // Cartesian goal - const moveit::core::LinkModel* link; - Eigen::Isometry3d target_eigen; - - // Cartesian targets require an IK reference frame + // Where to go? + Eigen::Isometry3d target; + // What frame+offset in the robot should go there? geometry_msgs::PoseStamped ik_pose_msg; + const boost::any& value = props.get("ik_frame"); + + auto get_tip{ [&jmg](std::string& out) { + // determine IK frame from group + std::vector tips; + jmg->getEndEffectorTips(tips); + if (tips.size() != 1) { + return false; + } + out = tips[0]; + return true; + } }; + if (value.empty()) { // property undefined - // determine IK link from group - if (!(link = jmg->getOnlyOneEndEffectorTip())) { + if (!get_tip(ik_pose_msg.header.frame_id)) { solution.markAsFailure("missing ik_frame"); return false; } - ik_pose_msg.header.frame_id = link->getName(); ik_pose_msg.pose.orientation.w = 1.0; } else { ik_pose_msg = boost::any_cast(value); - if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { - solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id); + if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) { + solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); + return false; + } else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) { + std::stringstream ss; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + solution.markAsFailure(ss.str()); return false; } } - if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) { + Eigen::Isometry3d ik_pose_world{ [&]() { + Eigen::Isometry3d t; + tf2::fromMsg(ik_pose_msg.pose, t); + t = scene->getFrameTransform(ik_pose_msg.header.frame_id) * t; + return t; + }() }; + + if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) { solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); return false; } + auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) { + geometry_msgs::PoseStamped msg; + msg.header.frame_id = scene->getPlanningFrame(); + msg.pose = tf2::toMsg(pose); + rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name); + } }; + // visualize plan with frame at target pose and frame at link - geometry_msgs::PoseStamped target; - target.header.frame_id = scene->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); - rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + add_frame(target, "target frame"); + add_frame(ik_pose_world, "ik frame"); + + const moveit::core::LinkModel* parent { +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id) +#else + getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id) +#endif + }; // transform target pose such that ik frame will reach there if link does Eigen::Isometry3d ik_pose; tf2::fromMsg(ik_pose_msg.pose, ik_pose); - target_eigen = target_eigen * ik_pose.inverse(); + target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName()); // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints); } // store result diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 36471132..a5daf365 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -126,6 +126,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) aco.object.primitive_poses[0].orientation.w = 1.0; #endif #if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_poses.resize(1, []() { geometry_msgs::Pose p; @@ -136,7 +137,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) return aco; } -TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) { +TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { const std::string ATTACHED_OBJECT{ "attached_object" }; scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); @@ -145,7 +146,9 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) { EXPECT_ONE_SOLUTION; } -TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) { +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK +// If we don't have this, we also don't have subframe support +TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; @@ -155,6 +158,7 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) { move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); EXPECT_ONE_SOLUTION; } +#endif int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From ef27a6eb22f1be68debbf77baa3b1547526f6fb2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 Oct 2021 14:17:33 +0200 Subject: [PATCH 07/12] ComputeIK supports attached-object ik frame --- core/src/stages/compute_ik.cpp | 38 +++++++++++++--------------------- 1 file changed, 14 insertions(+), 24 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index b86450cb..f3fe62d8 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -287,31 +287,21 @@ void ComputeIK::compute() { ik_pose_msg = boost::any_cast(value); Eigen::Isometry3d ik_pose; tf2::fromMsg(ik_pose_msg.pose, ik_pose); - if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) { - link = robot_model->getLinkModel(ik_pose_msg.header.frame_id); - } else { - const robot_state::AttachedBody* attached = - scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id); - if (!attached) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id); - return; - } - const EigenSTL::vector_Isometry3d& tf = -#if MOVEIT_HAS_OBJECT_POSE - attached->getShapePosesInLinkFrame(); -#else - attached->getFixedTransforms(); -#endif - if (tf.empty()) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes."); - return; - } - // prepend link - link = attached->getAttachedLink(); - ik_pose = tf[0] * ik_pose; + + if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { + ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + return; } + ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; + +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); +#else + link = getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); +#endif + // transform target pose such that ik frame will reach there if link does - target_pose = target_pose * ik_pose.inverse(); + target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName()); } // validate placed link for collisions @@ -323,7 +313,7 @@ void ComputeIK::compute() { // markers used for failures std::deque failure_markers; // frames at target pose and ik frame - rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); // visualize placed end-effector auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { From 01d0cf9a13241aa088fb89d3b0bd49901680f643 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Nov 2021 17:50:06 +0100 Subject: [PATCH 08/12] Extend mtc_add_test() macro to handle rostest as well --- core/test/CMakeLists.txt | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 0b530ed1..808e7b0d 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -9,17 +9,29 @@ if (CATKIN_ENABLE_TESTING) add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp) target_link_libraries(gtest_utils ${PROJECT_NAME}) - macro(mtc_add_test TYPE FILE) - string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${FILE}) - string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) - _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${FILE}) - target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages ${ARGN} gtest_utils gtest_main) + macro(mtc_add_test TYPE) + # Split ARGN into .test file and other sources + set(TEST_FILE ${ARGN}) + set(SOURCES ${ARGN}) + list(FILTER TEST_FILE INCLUDE REGEX "\.test$") + list(FILTER SOURCES EXCLUDE REGEX "\.test$") + # Determine TARGET name from first source file + list(GET SOURCES 0 TEST_NAME) + string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME}) + string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) + # Configure build target + if(TEST_FILE) # Add rostest if .test file was specified + _add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES}) + else() + _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + endif() + target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) endmacro() macro(mtc_add_gtest) - mtc_add_test("gtest" ${ARGN}) + mtc_add_test("gtest" ${ARGN}) endmacro() macro(mtc_add_gmock) - mtc_add_test("gmock" ${ARGN}) + mtc_add_test("gmock" ${ARGN}) endmacro() mtc_add_gtest(test_stage.cpp) @@ -33,8 +45,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) - add_rostest_gtest(${PROJECT_NAME}-test-move-to move_to.test test_move_to.cpp) - target_link_libraries(${PROJECT_NAME}-test-move-to ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) + mtc_add_gtest(test_move_to.cpp move_to.test) # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) From 0f53db5f12c349236e488a9e7e5647b7460502c4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 10 Nov 2021 13:33:26 +0100 Subject: [PATCH 09/12] simplify parameter-free lambdas I just didn't know the syntax was allowed --- core/test/test_move_to.cpp | 6 +++--- visualization/motion_planning_tasks/src/pluginlib_factory.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index a5daf365..1fbc512b 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -62,7 +62,7 @@ TEST_F(PandaMoveTo, mapTarget) { } TEST_F(PandaMoveTo, stateTarget) { - move_to->setGoal([]() { + move_to->setGoal([] { moveit_msgs::RobotState state; state.is_diff = true; state.joint_state.name = { "panda_joint1", "panda_joint2" }; @@ -110,7 +110,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) aco.object.header.frame_id = aco.link_name; aco.object.operation = aco.object.ADD; aco.object.id = id; - aco.object.primitives.resize(1, []() { + aco.object.primitives.resize(1, [] { shape_msgs::SolidPrimitive p; p.type = p.SPHERE; p.dimensions.resize(1); @@ -128,7 +128,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) #if MOVEIT_HAS_STATE_RIGID_PARENT_LINK // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); - aco.object.subframe_poses.resize(1, []() { + aco.object.subframe_poses.resize(1, [] { geometry_msgs::Pose p; p.orientation.w = 1.0; return p; diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index a5992724..77b91206 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -150,7 +150,7 @@ public: } template void addBuiltInClass(const QString& name, const QString& description) { - addBuiltInClass("Built Ins", name, description, []() { return new Derived(); }); + addBuiltInClass("Built Ins", name, description, [] { return new Derived(); }); } /** @brief Instantiate and return a instance of a subclass of Type using our From 48959c6806167f9d9f600530b6b588c99e2ee604 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 10 Nov 2021 14:33:56 +0100 Subject: [PATCH 10/12] add compat header to cmake previous oversight --- core/src/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 263c5033..85870be6 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/introspection.h ${PROJECT_INCLUDE}/marker_tools.h ${PROJECT_INCLUDE}/merge.h + ${PROJECT_INCLUDE}/moveit_compat.h ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h From f1fc447e3bc66c21fc8116a2de9aede1b2cdf16e Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 10 Nov 2021 14:36:58 +0100 Subject: [PATCH 11/12] establish utils namespace leaves us a place to put free helper functions --- .../moveit/task_constructor/moveit_compat.h | 15 ----- core/include/moveit/task_constructor/stage.h | 2 +- core/include/moveit/task_constructor/utils.h | 22 ++++++- core/src/CMakeLists.txt | 1 + core/src/stages/compute_ik.cpp | 6 +- core/src/stages/move_to.cpp | 10 +-- core/src/utils.cpp | 64 +++++++++++++++++++ 7 files changed, 90 insertions(+), 30 deletions(-) create mode 100644 core/src/utils.cpp diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index bf67ac07..91e7d12b 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -57,18 +57,3 @@ #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) #define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) - -#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK -#include -inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame) { - const moveit::core::LinkModel* link{ nullptr }; - - if (state.hasAttachedBody(frame)) { - link = state.getAttachedBody(frame)->getAttachedLink(); - } else if (state.getRobotModel()->hasLinkModel(frame)) - link = state.getLinkModel(frame); - - return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); -} -#endif diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 79c27837..7a25fa4b 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -84,7 +84,7 @@ enum InterfaceFlag GENERATE = WRITES_PREV_END | WRITES_NEXT_START, }; -using InterfaceFlags = Flags; +using InterfaceFlags = utils::Flags; /** invert interface such that * - new end can connect to old start diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index ed83596a..fd06c9a3 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -32,15 +32,27 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke - Desc: Project-agnostic utility classes +/* Authors: Robert Haschke, Michael 'v4hn' Goerner + Desc: Miscellaneous utilities */ #pragma once +#include #include #include +namespace moveit { + +namespace core { +class LinkModel; +class RobotState; +} // namespace core + +namespace task_constructor { + +namespace utils { + /** template class to compose flags from enums in a type-safe fashion */ template class Flags @@ -118,3 +130,9 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; + +const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, + std::string frame); +} // namespace utils +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 85870be6..d37ea27c 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -28,6 +28,7 @@ add_library(${PROJECT_NAME} stage.cpp storage.cpp task.cpp + utils.cpp solvers/planner_interface.cpp solvers/cartesian_path.cpp diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index f3fe62d8..0e5a6b55 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -294,11 +294,7 @@ void ComputeIK::compute() { } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); -#else - link = getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); -#endif + link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); // transform target pose such that ik frame will reach there if link does target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName()); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f24a908d..560fce75 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include @@ -262,13 +263,8 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - const moveit::core::LinkModel* parent { -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id) -#else - getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id) -#endif - }; + const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), + ik_pose_msg.header.frame_id) }; // transform target pose such that ik frame will reach there if link does Eigen::Isometry3d ik_pose; diff --git a/core/src/utils.cpp b/core/src/utils.cpp new file mode 100644 index 00000000..b008349a --- /dev/null +++ b/core/src/utils.cpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Hamburg University + * Copyright (c) 2017, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke */ + +#include + +#include + +namespace moveit { +namespace task_constructor { +namespace utils { + +const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, + std::string frame) { +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + return state.getRigidlyConnectedParentLinkModel(frame); +#else + const moveit::core::LinkModel* link{ nullptr }; + + if (state.hasAttachedBody(frame)) { + link = state.getAttachedBody(frame)->getAttachedLink(); + } else if (state.getRobotModel()->hasLinkModel(frame)) + link = state.getLinkModel(frame); + + return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); +#endif +} + +} // namespace utils +} // namespace task_constructor +} // namespace moveit From 3b835986e34e09edf1e2f93680f0b6a5d4ace38b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 12 Nov 2021 14:02:46 +0100 Subject: [PATCH 12/12] refactor logic to handle ik_frame fallbacks and verification. --- core/include/moveit/task_constructor/utils.h | 19 +++++++- core/src/stages/move_relative.cpp | 25 ++-------- core/src/stages/move_to.cpp | 51 +++----------------- core/src/utils.cpp | 49 +++++++++++++++++++ 4 files changed, 77 insertions(+), 67 deletions(-) diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index fd06c9a3..302d4780 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -42,14 +42,25 @@ #include #include +#include + +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene); +} + namespace moveit { namespace core { -class LinkModel; -class RobotState; +MOVEIT_CLASS_FORWARD(LinkModel); +MOVEIT_CLASS_FORWARD(JointModelGroup); +MOVEIT_CLASS_FORWARD(RobotState); } // namespace core namespace task_constructor { +MOVEIT_CLASS_FORWARD(Property); +MOVEIT_CLASS_FORWARD(SolutionBase); namespace utils { @@ -133,6 +144,10 @@ private: const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, std::string frame); + +bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, + const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); } // namespace utils } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 4271337d..dbf5ab7f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -193,24 +193,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { // Cartesian targets require an IK reference frame - geometry_msgs::PoseStamped ik_pose_msg; const moveit::core::LinkModel* link; - const boost::any& value = props.get("ik_frame"); - if (value.empty()) { // property undefined - // determine IK link from group - if (!(link = jmg->getOnlyOneEndEffectorTip())) { - solution.markAsFailure("missing ik_frame"); - return false; - } - ik_pose_msg.header.frame_id = link->getName(); - ik_pose_msg.pose.orientation.w = 1.0; - } else { - ik_pose_msg = boost::any_cast(value); - if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { - solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id); - return false; - } - } + Eigen::Isometry3d ik_pose_world; + + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + return false; bool use_rotation_distance = false; // measure achieved distance as rotation? Eigen::Vector3d linear; // linear translation @@ -291,9 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning COMPUTE: // transform target pose such that ik frame will reach there if link does - Eigen::Isometry3d ik_pose; - tf2::fromMsg(ik_pose_msg.pose, ik_pose); - target_eigen = target_eigen * ik_pose.inverse(); + target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 560fce75..3270a831 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -208,44 +208,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP // What frame+offset in the robot should go there? geometry_msgs::PoseStamped ik_pose_msg; - const boost::any& value = props.get("ik_frame"); - - auto get_tip{ [&jmg](std::string& out) { - // determine IK frame from group - std::vector tips; - jmg->getEndEffectorTips(tips); - if (tips.size() != 1) { - return false; - } - out = tips[0]; - return true; - } }; - - if (value.empty()) { // property undefined - if (!get_tip(ik_pose_msg.header.frame_id)) { - solution.markAsFailure("missing ik_frame"); - return false; - } - ik_pose_msg.pose.orientation.w = 1.0; - } else { - ik_pose_msg = boost::any_cast(value); - if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) { - solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); - return false; - } else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) { - std::stringstream ss; - ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; - solution.markAsFailure(ss.str()); - return false; - } - } - - Eigen::Isometry3d ik_pose_world{ [&]() { - Eigen::Isometry3d t; - tf2::fromMsg(ik_pose_msg.pose, t); - t = scene->getFrameTransform(ik_pose_msg.header.frame_id) * t; - return t; - }() }; + const moveit::core::LinkModel* link; + Eigen::Isometry3d ik_pose_world; + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + return false; if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) { solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); @@ -263,16 +229,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), - ik_pose_msg.header.frame_id) }; - // transform target pose such that ik frame will reach there if link does - Eigen::Isometry3d ik_pose; - tf2::fromMsg(ik_pose_msg.pose, ik_pose); - target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName()); + target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); // plan to Cartesian target - success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); } // store result diff --git a/core/src/utils.cpp b/core/src/utils.cpp index b008349a..0a4e7e0e 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -35,9 +35,14 @@ /* Authors: Michael Goerner, Robert Haschke */ +#include + #include +#include #include +#include +#include namespace moveit { namespace task_constructor { @@ -59,6 +64,50 @@ const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit:: #endif } +bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, + const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { + auto get_tip = [&jmg]() -> const moveit::core::LinkModel* { + // determine IK frame from group + std::vector tips; + jmg->getEndEffectorTips(tips); + if (tips.size() != 1) { + return nullptr; + } + return tips[0]; + }; + + if (property.value().empty()) { // property undefined + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("missing ik_frame"); + return false; + } + tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } else { + auto ik_pose_msg = boost::any_cast(property.value()); + if (ik_pose_msg.header.frame_id.empty()) { + if (!(robot_link = get_tip())) { + solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); + return false; + } + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; + } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { + robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id); + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; + } else { + std::stringstream ss; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + solution.markAsFailure(ss.str()); + return false; + } + } + + return true; +} + } // namespace utils } // namespace task_constructor } // namespace moveit