diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index c813f8fb..00948ca2 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -105,7 +105,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->properties().configureInitFrom(Stage::PARENT); - gengrasp->setGripperGraspPose("open"); + gengrasp->setNamedPose("open"); gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)* Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index e0cf26ad..340b4b42 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -1,11 +1,8 @@ #include #include -#include -#include -#include -#include -#include +#include +#include #include #include @@ -44,78 +41,31 @@ int main(int argc, char** argv){ Task t; Stage* initial_stage = nullptr; + auto initial = std::make_unique("current state"); + initial_stage = initial.get(); + t.add(std::move(initial)); - { - auto initial = std::make_unique("current state"); - initial_stage = initial.get(); - t.add(std::move(initial)); - } + auto grasp_generator = std::make_unique(); + grasp_generator->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + grasp_generator->setAngleDelta(.2); + grasp_generator->setPreGraspPose("open"); + grasp_generator->setGraspPose("closed"); + grasp_generator->setMonitoredStage(initial_stage); - { - auto move= std::make_unique("open gripper"); - move->setEndEffector("left_gripper"); - move->setTo("open"); - t.add(std::move(move)); - } + auto pick = std::make_unique(std::move(grasp_generator)); + pick->setProperty("eef", std::string("left_gripper")); + pick->setProperty("object", std::string("object")); + geometry_msgs::TwistStamped approach; + approach.header.frame_id = "l_gripper_tool_frame"; + approach.twist.linear.x = 1.0; + pick->setApproachMotion(approach, 0.03, 0.1); - { - auto move= std::make_unique("move to pre-grasp"); - move->setGroup("left_arm"); - move->setPlannerId("RRTConnectkConfigDefault"); - move->setTimeout(8.0); - t.add(std::move(move)); - } + geometry_msgs::TwistStamped lift; + lift.header.frame_id = "base_link"; + lift.twist.linear.z = 1.0; + pick->setLiftMotion(lift, 0.03, 0.05); - { - auto move= std::make_unique("proceed to grasp pose"); - move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1)); - move->setGroup("left_arm"); - move->setLink("l_gripper_tool_frame"); - move->setMinMaxDistance(.03, 0.1); - move->setCartesianStepSize(0.02); - - geometry_msgs::PointStamped target; - target.header.frame_id= "object"; - move->towards(target); - t.add(std::move(move)); - } - - { - auto gengrasp= std::make_unique("generate grasp pose"); - gengrasp->setEndEffector("left_gripper"); - gengrasp->setGripperGraspPose("open"); - gengrasp->setObject("object"); - gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); - gengrasp->setAngleDelta(.2); - gengrasp->setMonitoredStage(initial_stage); - - auto ik = std::make_unique("compute ik", std::move(gengrasp)); - ik->setEndEffector("left_gripper"); - t.add(std::move(ik)); - } - - { - auto move= std::make_unique("grasp"); - move->setEndEffector("left_gripper"); - move->setAttachLink("l_gripper_tool_frame"); - move->setTo("closed"); - move->graspObject("object"); - t.add(std::move(move)); - } - - { - auto move= std::make_unique("lift object"); - move->setGroup("left_arm"); - move->setLink("l_gripper_tool_frame"); - move->setMinMaxDistance(0.03, 0.05); - move->setCartesianStepSize(0.01); - - geometry_msgs::Vector3Stamped direction; - direction.header.frame_id= "base_link"; - direction.vector.z= 1.0; - move->along(direction); - t.add(std::move(move)); - } + t.add(std::move(pick)); try { t.plan(); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 92e2e1f6..c18f35a9 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -84,7 +84,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->properties().configureInitFrom(Stage::PARENT); - gengrasp->setGripperGraspPose("open"); + gengrasp->setNamedPose("open"); gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 9f4b4885..d6559312 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -52,7 +53,7 @@ public: bool compute() override; void setEndEffector(const std::string &eef); - void setGripperGraspPose(const std::string &pose_name); + void setNamedPose(const std::string &pose_name); void setObject(const std::string &object); void setToolToGraspTF(const geometry_msgs::TransformStamped &transform); diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index a30e1c7c..b92aaebf 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -60,7 +60,7 @@ namespace moveit { namespace task_constructor { namespace stages { class ModifyPlanningScene : public PropagatingEitherWay { public: typedef std::vector Names; - typedef std::function ApplyCallback; + typedef std::function ApplyCallback; ModifyPlanningScene(const std::string& name = "modify planning scene"); bool computeForward(const InterfaceState& from) override; diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h new file mode 100644 index 00000000..c1ebd54e --- /dev/null +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +/** Pick wraps a complete pipeline to pick up an object with a given end effector. + * + * Picking consist of the following sub stages: + * - reaching to the object + "pre-grasp" end effector posture + * - linearly approaching the object along an approach direction/twist + * - "grasp" end effector posture + * - attach object + * - lift along along a given direction/twist + * + * The end effector postures corresponding to pre-grasp and grasp as well as + * the end effector's Cartesian pose needs to be provided by an external grasp stage. + */ +class Pick : public SerialContainer { + Stage* grasp_stage_ = nullptr; + Stage* approach_stage_ = nullptr; + Stage* lift_stage_ = nullptr; + +public: + Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick"); + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + void setEndEffector(const std::string& eef) { + properties().set("eef", eef); + } + void setObject(const std::string& object) { + properties().set("object", object); + } + + void setApproachMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); + void setLiftMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h new file mode 100644 index 00000000..160ff4e1 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } } +namespace moveit { namespace task_constructor { namespace stages { + +class GenerateGraspPose; + +/** Simple Grasp Stage + * + * Given a named pre-grasp and grasp posture the stage generates + * fully-qualified pre-grasp and grasp robot states, connected + * by a grasping trajectory of the end-effector. + */ +class SimpleGrasp : public SerialContainer { + moveit::core::RobotModelConstPtr model_; + GenerateGraspPose* grasp_generator_ = nullptr; + +public: + SimpleGrasp(const std::string& name = "grasp"); + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + void setMonitoredStage(Stage* monitored); + + void setEndEffector(const std::string& eef) { + properties().set("eef", eef); + } + void setObject(const std::string& object) { + properties().set("object", object); + } + + void setPreGraspPose(const std::string& pregrasp) { + properties().set("pregrasp", pregrasp); + } + void setGraspPose(const std::string& grasp) { + properties().set("grasp", grasp); + } + + void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) { + properties().set("tool_to_grasp_tf", transform); + } + void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = ""); + template + void setToolToGraspTF(const T& t, const std::string& link = "") { + Eigen::Affine3d transform; transform = t; + setToolToGraspTF(transform, link); + } + + void setAngleDelta(double angle_delta) { + properties().set("angle_delta", angle_delta); + } + + void setMaxIKSolutions(uint32_t max_ik_solutions) { + properties().set("max_ik_solutions", max_ik_solutions); + } + void setTimeout(double timeout) { + properties().set("timeout", timeout); + } +}; + +} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 02915ad2..03959900 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -11,6 +11,9 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/move_to.h ${PROJECT_INCLUDE}/stages/move_relative.h + ${PROJECT_INCLUDE}/stages/simple_grasp.h + ${PROJECT_INCLUDE}/stages/pick.h + ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/gripper.h ${PROJECT_INCLUDE}/stages/move.h @@ -27,6 +30,9 @@ add_library(${PROJECT_NAME}_stages move_to.cpp move_relative.cpp + simple_grasp.cpp + pick.cpp + cartesian_position_motion.cpp gripper.cpp move.cpp diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 6bd164a3..ddf17d38 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -50,8 +50,8 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : MonitoringGenerator(name) { auto& p = properties(); - p.declare("eef", "name of end-effector group"); - p.declare("eef_named_pose"); + p.declare("eef", "name of end-effector"); + p.declare("pregrasp", "name of end-effector's pregrasp pose"); p.declare("object"); p.declare("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame"); p.declare("angle_delta", 0.1, "angular steps (rad)"); @@ -61,8 +61,8 @@ void GenerateGraspPose::setEndEffector(const std::string &eef){ setProperty("eef", eef); } -void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){ - setProperty("eef_named_pose", pose_name); +void GenerateGraspPose::setNamedPose(const std::string &pose_name){ + setProperty("pregrasp", pose_name); } void GenerateGraspPose::setObject(const std::string &object){ @@ -111,11 +111,13 @@ bool GenerateGraspPose::compute(){ const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef); robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst(); - const std::string& eef_named_pose = props.get("eef_named_pose"); - robot_state.setToDefaultValues(jmg , eef_named_pose); + const std::string& joint_pose = props.get("pregrasp"); + if(!joint_pose.empty()){ + robot_state.setToDefaultValues(jmg , joint_pose); + } geometry_msgs::TransformStamped tool2grasp_msg = props.get("tool_to_grasp_tf"); - const std::string &link_name = jmg ->getEndEffectorParentGroup().second; + const std::string &link_name = jmg->getEndEffectorParentGroup().second; if (tool2grasp_msg.header.frame_id.empty()) // if no frame_id is given tool2grasp_msg.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame Eigen::Affine3d to_grasp; diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp new file mode 100644 index 00000000..0fdae2c6 --- /dev/null +++ b/core/src/stages/pick.cpp @@ -0,0 +1,118 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace moveit { namespace task_constructor { namespace stages { + +Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) + : SerialContainer(name) +{ + PropertyMap& p = properties(); + p.declare("eef", "end effector name"); + p.declare("object", "name of object to grasp"); + // internal properties (cannot be marked as such yet) + p.declare("eef_group", "JMG of eef"); + p.declare("eef_parent_group", "JMG of eef's parent"); + + auto cartesian = std::make_shared(); + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + + { + auto move = std::make_unique("open gripper", pipeline); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); + move->setGoal("open"); // TODO: retrieve from grasp stage + insert(std::move(move)); + } + + { + auto move = std::make_unique("move to object", pipeline); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + insert(std::move(move)); + } + + { + auto approach = std::make_unique("approach object", cartesian); + PropertyMap& p = approach->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.set("marker_ns", std::string("approach")); + approach_stage_ = approach.get(); + insert(std::move(approach)); + } + + grasp_stage_ = grasp_stage.get(); + grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); + insert(std::move(grasp_stage)); + + { + auto attach = std::make_unique("attach object"); + PropertyMap& p = attach->properties(); + p.declare("eef"); + p.declare("object"); + p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + const std::string& eef = p.get("eef"); + moveit_msgs::AttachedCollisionObject obj; + obj.object.operation = moveit_msgs::CollisionObject::ADD; + obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; + obj.object.id = p.get("object"); + scene->processAttachedCollisionObjectMsg(obj); + }); + insert(std::move(attach)); + } + + { + auto lift = std::make_unique("lift object", cartesian); + PropertyMap& p = lift->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.set("marker_ns", std::string("lift")); + lift_stage_ = lift.get(); + insert(std::move(lift)); + } +} + +void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) +{ + // inherit properties from parent + PropertyMap* p = &properties(); + p->performInitFrom(Stage::PARENT, parent()->properties()); + + // init internal properties + const std::string &eef = p->get("eef"); + const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef); + p->set("eef_group", jmg->getName()); + p->set("eef_parent_group", jmg->getEndEffectorParentGroup().first); + + // propagate my properties to children (and do standard init) + SerialContainer::init(robot_model); +} + +void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +{ + auto& p = approach_stage_->properties(); + p.set("twist", motion); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +{ + auto& p = lift_stage_->properties(); + p.set("twist", motion); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +} } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp new file mode 100644 index 00000000..64ba316e --- /dev/null +++ b/core/src/stages/simple_grasp.cpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +SimpleGrasp::SimpleGrasp(const std::string& name) + : SerialContainer(name) +{ + { + auto gengrasp = std::make_unique("generate grasp pose"); + grasp_generator_ = gengrasp.get(); + + auto ik = std::make_unique("compute ik", std::move(gengrasp)); + const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta", "tool_to_grasp_tf" }; + ik->exposePropertiesOfChild(0, grasp_prop_names); + insert(std::move(ik)); + + exposePropertiesOfChild(-1, grasp_prop_names); + exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" }); + } + { + auto allow_touch = std::make_unique("enable object collision"); + PropertyMap& p = allow_touch->properties(); + p.declare("eef"); + p.declare("object"); + p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + + allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst(); + const std::string& eef = p.get("eef"); + const std::string& object = p.get("object"); + acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) + ->getLinkModelNamesWithCollisionGeometry(), true); + }); + insert(std::move(allow_touch)); + } + { + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + + auto move = std::make_unique("close gripper", pipeline); + move->restrictDirection(MoveTo::FORWARD); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ + const std::string& eef = parent_map.get("eef"); + const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef); + return boost::any(jmg->getName()); + }); + insert(std::move(move)); + exposePropertyOfChildAs(-1, "joint_pose", "grasp"); + } +} + +void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model) +{ + model_ = robot_model; + SerialContainer::init(robot_model); +} + +void SimpleGrasp::setMonitoredStage(Stage* monitored) +{ + grasp_generator_->setMonitoredStage(monitored); +} + +void SimpleGrasp::setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link) { + geometry_msgs::TransformStamped stamped; + stamped.header.frame_id = link; + stamped.child_frame_id = "grasp_frame"; + tf::transformEigenToMsg(transform, stamped.transform); + setToolToGraspTF(stamped); +} + +} } }