From e1216aa8ab3214f8ef5aada37bbea38b5be41c62 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 Oct 2021 15:52:15 +0200 Subject: [PATCH] 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);