diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 5f903f0b..2e6e9207 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -46,18 +46,20 @@ namespace moveit { namespace task_constructor { namespace stages { class GenerateGraspPose; -/** Simple Grasp Stage +/** base class for simple grasping / ungrasping * - * 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. + * 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. + * + * Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class + * provides the common functionality for grasping (forward = true) and ungrasping (forward = false). */ -class SimpleGrasp : public SerialContainer { +class SimpleGraspBase : public SerialContainer { moveit::core::RobotModelConstPtr model_; GenerateGraspPose* grasp_generator_ = nullptr; public: - SimpleGrasp(const std::string& name = "grasp"); + SimpleGraspBase(const std::string& name, bool forward); void init(const moveit::core::RobotModelConstPtr& robot_model) override; void setMonitoredStage(Stage* monitored); @@ -101,4 +103,18 @@ public: } }; + +/// specialization of SimpleGraspBase to realize grasping +class SimpleGrasp : public SimpleGraspBase { +public: + SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {} +}; + + +/// specialization of SimpleGraspBase to realize ungrasping +class SimpleUnGrasp : public SimpleGraspBase { +public: + SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {} +}; + } } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 1eb919fd..114ec0a4 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -49,23 +49,24 @@ namespace moveit { namespace task_constructor { namespace stages { -SimpleGrasp::SimpleGrasp(const std::string& name) +SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) : SerialContainer(name) { + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - auto gengrasp = std::make_unique("generate grasp pose"); + auto gengrasp = std::make_unique(forward ? "generate grasp pose" : "generate release 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" }; ik->exposePropertiesOfChild(0, grasp_prop_names); - insert(std::move(ik)); + insert(std::move(ik), insertion_position); - exposePropertiesOfChild(-1, grasp_prop_names); - exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout", "ik_frame" }); + exposePropertiesOfChild(insertion_position, grasp_prop_names); + exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { - auto allow_touch = std::make_unique("allow object collision"); + auto allow_touch = std::make_unique(forward ? "allow object collision" : "forbid object collision"); PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); @@ -78,53 +79,54 @@ SimpleGrasp::SimpleGrasp(const std::string& name) acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) ->getLinkModelNamesWithCollisionGeometry(), true); }); - insert(std::move(allow_touch)); + insert(std::move(allow_touch), insertion_position); } { auto pipeline = std::make_shared(); pipeline->setTimeout(8.0); pipeline->setPlannerId("RRTConnectkConfigDefault"); - auto move = std::make_unique("close gripper", pipeline); + auto move = std::make_unique(forward ? "close gripper" : "open gripper", pipeline); 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"); + insert(std::move(move), insertion_position); + exposePropertyOfChildAs(insertion_position, "joint_pose", forward ? "grasp" : "pregrasp"); } { - auto attach = std::make_unique("attach object"); + auto attach = std::make_unique(forward ? "attach object" : "detach 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){ + attach->setCallback([this, forward](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.object.operation = forward ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); }); - insert(std::move(attach)); + insert(std::move(attach), insertion_position); } } -void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model) +void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) { model_ = robot_model; SerialContainer::init(robot_model); } -void SimpleGrasp::setMonitoredStage(Stage* monitored) +void SimpleGraspBase::setMonitoredStage(Stage* monitored) { grasp_generator_->setMonitoredStage(monitored); } -void SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { +void SimpleGraspBase::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);