diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 0fdae2c6..800b95c4 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -56,23 +56,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) 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(); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 64ba316e..74c25fa2 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -86,7 +86,6 @@ SimpleGrasp::SimpleGrasp(const std::string& name) 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"); @@ -96,6 +95,22 @@ SimpleGrasp::SimpleGrasp(const std::string& name) insert(std::move(move)); exposePropertyOfChildAs(-1, "joint_pose", "grasp"); } + { + 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)); + } } void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)