From 3b835986e34e09edf1e2f93680f0b6a5d4ace38b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 12 Nov 2021 14:02:46 +0100 Subject: [PATCH] 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