diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 5ad0c6c2..7a71156c 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -46,7 +46,6 @@ int main(int argc, char** argv){ t.setProperty("group", std::string("left_arm")); t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - t.setProperty("link", std::string("la_tool_mount")); auto pipeline = std::make_shared(); pipeline->setTimeout(8.0); @@ -87,7 +86,7 @@ int main(int argc, char** argv){ move->restrictDirection(stages::MoveRelative::BACKWARD); move->properties().configureInitFrom(Stage::PARENT); move->properties().set("marker_ns", std::string("approach")); - move->properties().set("link", std::string("lh_tool_frame")); + move->setIKFrame("lh_tool_frame"); move->setMinMaxDistance(0.05, 0.1); geometry_msgs::Vector3Stamped direction; @@ -142,7 +141,7 @@ int main(int argc, char** argv){ move->properties().configureInitFrom(Stage::PARENT, {"group"}); move->setMinMaxDistance(0.03, 0.05); move->properties().set("marker_ns", std::string("lift")); - move->properties().set("link", std::string("lh_tool_frame")); + move->setIKFrame("lh_tool_frame"); geometry_msgs::Vector3Stamped direction; direction.header.frame_id = "world"; @@ -156,7 +155,7 @@ int main(int argc, char** argv){ move->properties().configureInitFrom(Stage::PARENT, {"group"}); move->setMinMaxDistance(0.1, 0.2); move->properties().set("marker_ns", std::string("lift")); - move->properties().set("link", std::string("lh_tool_frame")); + move->setIKFrame("lh_tool_frame"); geometry_msgs::TwistStamped twist; twist.header.frame_id = "object"; diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 91a872a6..0a1a51a1 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -92,6 +92,4 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; }; -std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg); - } } } diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index d9a1d062..c98fed94 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -58,8 +58,18 @@ public: void setGroup(const std::string& group) { setProperty("group", group); } - void setLink(const std::string& link) { - setProperty("link", link); + /// setters for IK frame + void setIKFrame(const geometry_msgs::PoseStamped &pose) { + setProperty("ik_frame", pose); + } + void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); + template + void setIKFrame(const T& p, const std::string& link) { + Eigen::Affine3d pose; pose = p; + setIKFrame(pose, link); + } + void setIKFrame(const std::string& link) { + setIKFrame(Eigen::Affine3d::Identity(), link); } /// set minimum / maximum distance to move diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 6b0190f4..c50e9700 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -60,8 +60,18 @@ public: void setGroup(const std::string& group) { setProperty("group", group); } - void setLink(const std::string& link) { - setProperty("link", link); + /// setters for IK frame + void setIKFrame(const geometry_msgs::PoseStamped &pose) { + setProperty("ik_frame", pose); + } + void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); + template + void setIKFrame(const T& p, const std::string& link) { + Eigen::Affine3d pose; pose = p; + setIKFrame(pose, link); + } + void setIKFrame(const std::string& link) { + setIKFrame(Eigen::Affine3d::Identity(), link); } /// move link to given pose diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 4381bc24..da0a9fdd 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -25,7 +25,6 @@ add_library(${PROJECT_NAME} storage.cpp task.cpp - solvers/planner_interface.cpp solvers/cartesian_path.cpp solvers/pipeline_planner.cpp ) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e49032cb..70751408 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -63,9 +63,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { - const moveit::core::LinkModel* link; - const std::string& link_name = solvers::getEndEffectorLink(jmg); - if (!(link = from->getRobotModel()->getLinkModel(link_name))) { + const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); + if (!link) { ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); return false; } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp deleted file mode 100644 index 64cfa93b..00000000 --- a/core/src/solvers/planner_interface.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * 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: Robert Haschke - Desc: planner interface -*/ - -#include -#include - -namespace moveit { namespace task_constructor { namespace solvers { - -std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg) -{ - // TODO: directly return LinkModel* - const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); - return link ? link->getName() : std::string(); -} - -} } } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 6999a6b6..7b8f3b76 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Michael Goerner, Robert Haschke +/* Authors: Robert Haschke, Michael Goerner Desc: Move link along Cartesian direction */ @@ -49,9 +49,10 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf { auto& p = properties(); p.declare("timeout", 10.0, "planning timeout"); + // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" p.declare("marker_ns", "", "marker namespace"); p.declare("group", "name of planning group"); - p.declare("link", "", "link to move (default is tip of jmg)"); + p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("min_distance", -1.0, "minimum distance to move"); p.declare("max_distance", 0.0, "maximum distance to move"); @@ -63,6 +64,14 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf "constraints to maintain during trajectory"); } +void MoveRelative::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) +{ + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + tf::poseEigenToMsg(pose, pose_msg.pose); + setIKFrame(pose_msg); +} + void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) { PropagatingEitherWay::init(robot_model); @@ -72,12 +81,13 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, SubTrajectory &trajectory, Direction dir) { scene = state.scene()->diff(); - assert(scene->getRobotModel()); + const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + assert(robot_model); const auto& props = properties(); double timeout = props.get("timeout"); const std::string& group = props.get("group"); - const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (!jmg) { ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group); return false; @@ -105,7 +115,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning const auto& joints = boost::any_cast>(goal); for (const auto& j : joints) { int index = robot_state.getRobotModel()->getVariableIndex(j.first); - auto jm = scene->getRobotModel()->getJointModel(index); + auto jm = robot_model->getJointModel(index); if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) { ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'"); return false; @@ -116,15 +126,24 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning robot_state.update(); success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { - // Cartesian targets require the link name - // TODO: use ik_frame property as in ComputeIK - std::string link_name = props.get("link"); + // Cartesian targets require an IK reference frame + geometry_msgs::PoseStamped ik_pose_msg; const moveit::core::LinkModel* link; - if (link_name.empty()) - link_name = solvers::getEndEffectorLink(jmg); - if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { - ROS_WARN_STREAM_NAMED("MoveRelative", "No or invalid link name specified: " << link_name); - return false; + const boost::any& value = props.get("ik_frame"); + if (value.empty()) { // property undefined + // determine IK link from group + if (!(link = jmg->getOnlyOneEndEffectorTip())) { + ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link"); + 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))) { + ROS_WARN_STREAM_NAMED("MoveRelative", "Unknown link: " << ik_pose_msg.header.frame_id); + return false; + } } bool use_rotation_distance = false; // measure achieved distance as rotation? @@ -133,7 +152,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning double linear_norm = 0.0, angular_norm = 0.0; Eigen::Affine3d target_eigen; - Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success + Eigen::Affine3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success boost::any goal = props.get("twist"); if (!goal.empty()) { @@ -197,6 +216,11 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning target_eigen.translation() += linear; } + // transform target pose such that ik frame will reach there if link does + Eigen::Affine3d ik_pose; + tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose); + target_eigen = target_eigen * ik_pose.inverse(); + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); // min_distance reached? @@ -223,7 +247,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning // add an arrow marker visualization_msgs::Marker m; - // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" m.ns = props.get("marker_ns"); if (!m.ns.empty()) { m.header.frame_id = scene->getPlanningFrame(); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index e6f36c63..e2fc057b 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Michael Goerner, Robert Haschke +/* Authors: Robert Haschke, Michael Goerner Desc: Move to joint-state or Cartesian goal pose */ @@ -51,7 +51,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan auto& p = properties(); p.declare("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage p.declare("group", "name of planning group"); - p.declare("link", "", "link to move (default is tip of jmg)"); + p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("pose", "Cartesian target pose"); p.declare("point", "Cartesian target point"); @@ -62,6 +62,14 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan "constraints to maintain during trajectory"); } +void MoveTo::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) +{ + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + tf::poseEigenToMsg(pose, pose_msg.pose); + setIKFrame(pose_msg); +} + void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) { PropagatingEitherWay::init(robot_model); @@ -114,12 +122,13 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) { bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, SubTrajectory &solution, Direction dir) { scene = state.scene()->diff(); - assert(scene->getRobotModel()); + const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + assert(robot_model); const auto& props = properties(); double timeout = props.get("timeout"); const std::string& group = props.get("group"); - const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (!jmg) { ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group); return false; @@ -161,14 +170,23 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP const moveit::core::LinkModel* link; Eigen::Affine3d target_eigen; - // Cartesian targets require the link name - // TODO: use ik_frame property as in ComputeIK - std::string link_name = props.get("link"); - if (link_name.empty()) - link_name = solvers::getEndEffectorLink(jmg); - if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { - ROS_WARN_STREAM_NAMED("MoveTo", "No or invalid link name specified: " << link_name); - return false; + // Cartesian targets require an IK reference frame + geometry_msgs::PoseStamped ik_pose_msg; + const boost::any& value = props.get("ik_frame"); + if (value.empty()) { // property undefined + // determine IK link from group + if (!(link = jmg->getOnlyOneEndEffectorTip())) { + ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link"); + 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))) { + ROS_WARN_STREAM_NAMED("MoveTo", "Unknown link: " << ik_pose_msg.header.frame_id); + return false; + } } boost::any goal = props.get("pose"); @@ -184,10 +202,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); // frame at link - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.frame_id = link_name; - pose_msg.pose.orientation.w = 1.0; - rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); } goal = props.get("point"); @@ -201,10 +216,15 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP target_point = frame * target_point; // retain link orientation - target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name); + target_eigen = scene->getCurrentState().getGlobalLinkTransform(link); target_eigen.translation() = target_point; } + // transform target pose such that ik frame will reach there if link does + Eigen::Affine3d ik_pose; + tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose); + target_eigen = target_eigen * ik_pose.inverse(); + // plan to Cartesian target success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); } diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 4b418c5d..5f88698f 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -24,11 +24,22 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na cartesian_solver_ = std::make_shared(); int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order + auto init_ik_frame = [](const PropertyMap& other) -> boost::any { + geometry_msgs::PoseStamped pose; + const boost::any& frame = other.get("eef_frame"); + if (frame.empty()) + return boost::any(); + + pose.header.frame_id = boost::any_cast("eef_frame"); + pose.pose.orientation.w = 1.0; + return pose; + }; + { auto approach = std::make_unique(forward ? "approach object" : "retract", cartesian_solver_); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); - p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); + p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); p.set("marker_ns", std::string(forward ? "approach" : "retract")); approach_stage_ = approach.get(); insert(std::move(approach), insertion_position); @@ -42,7 +53,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na auto lift = std::make_unique(forward ? "lift object" : "place object", cartesian_solver_); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); - p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); + p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); p.set("marker_ns", std::string(forward ? "lift" : "place")); lift_stage_ = lift.get(); insert(std::move(lift), insertion_position);