From 92296e020f712b2f62203733063d60d38d75d8a7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 1 Jun 2018 10:13:18 +0200 Subject: [PATCH] SimpleGrasp(GraspGenerator) allow any GraspGenerator stage that provides "pregrasp" and "grasp" postures as well as a target_pose for grasping. --- core/demo/plan_pick_pr2.cpp | 15 +++--- core/demo/plan_pick_ur5.cpp | 17 ++++--- .../task_constructor/stages/simple_grasp.h | 38 ++++++++------- core/src/stages/simple_grasp.cpp | 47 ++++++++++++------- 4 files changed, 71 insertions(+), 46 deletions(-) diff --git a/core/demo/plan_pick_pr2.cpp b/core/demo/plan_pick_pr2.cpp index c6f36d1a..0166c2f1 100644 --- a/core/demo/plan_pick_pr2.cpp +++ b/core/demo/plan_pick_pr2.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -55,15 +56,17 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique(); - grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + auto grasp_generator = std::make_unique("generate grasp pose"); grasp_generator->setAngleDelta(.2); - grasp_generator->setPreGraspPose("open"); - grasp_generator->setGraspPose("closed"); - grasp_generator->setMonitoredStage(initial_stage); + + auto grasp = std::make_unique(std::move(grasp_generator)); + grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + grasp->setPreGraspPose("open"); + grasp->setGraspPose("closed"); + grasp->setMonitoredStage(initial_stage); // pick stage - auto pick = std::make_unique(std::move(grasp_generator)); + auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("left_gripper")); pick->setProperty("object", std::string("object")); geometry_msgs::TwistStamped approach; diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 3cd1919e..42ad3c2b 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -55,15 +56,17 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique(); - grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); - grasp_generator->setMaxIKSolutions(8); + auto grasp_generator = std::make_unique("generate grasp pose"); grasp_generator->setAngleDelta(.2); - grasp_generator->setPreGraspPose("open"); - grasp_generator->setGraspPose("closed"); - grasp_generator->setMonitoredStage(initial_stage); - auto pick = std::make_unique(std::move(grasp_generator)); + auto grasp = std::make_unique(std::move(grasp_generator)); + grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); + grasp->setMaxIKSolutions(8); + grasp->setPreGraspPose("open"); + grasp->setGraspPose("closed"); + grasp->setMonitoredStage(initial_stage); + + auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("gripper")); pick->setProperty("object", std::string("object")); geometry_msgs::TwistStamped approach; diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 2e6e9207..95676f06 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -48,34 +48,45 @@ class GenerateGraspPose; /** 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 pre-grasp and grasp posture the stage generates a trajectory + * connecting these two states by a grasping trajectory. + * The class takes a generator stage that provides the grasp pose and + * optionally the pre-grasp and grasp postures. * * 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 SimpleGraspBase : public SerialContainer { moveit::core::RobotModelConstPtr model_; - GenerateGraspPose* grasp_generator_ = nullptr; + MonitoringGenerator* generator_ = nullptr; + +protected: + void setup(std::unique_ptr&& generator, bool forward); public: - SimpleGraspBase(const std::string& name, bool forward); + SimpleGraspBase(const std::string& name); void init(const moveit::core::RobotModelConstPtr& robot_model) override; void setMonitoredStage(Stage* monitored); void setEndEffector(const std::string& eef) { - properties().set("eef", eef); + properties().set("eef", eef); } void setObject(const std::string& object) { - properties().set("object", object); + properties().set("object", object); } void setPreGraspPose(const std::string& pregrasp) { - properties().set("pregrasp", pregrasp); + properties().set("pregrasp", pregrasp); } void setGraspPose(const std::string& grasp) { - properties().set("grasp", grasp); + properties().set("grasp", grasp); + } + void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { + properties().set("pregrasp", pregrasp); + } + void setGraspPose(const moveit_msgs::RobotState& grasp) { + properties().set("grasp", grasp); } void setIKFrame(const geometry_msgs::PoseStamped &transform) { @@ -91,30 +102,23 @@ public: setIKFrame(Eigen::Affine3d::Identity(), 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); - } }; /// specialization of SimpleGraspBase to realize grasping class SimpleGrasp : public SimpleGraspBase { public: - SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {} + SimpleGrasp(std::unique_ptr&& generator, const std::string& name = "grasp"); }; /// specialization of SimpleGraspBase to realize ungrasping class SimpleUnGrasp : public SimpleGraspBase { public: - SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {} + SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name = "ungrasp"); }; } } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 7bc7ab75..5412e90e 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -36,7 +36,6 @@ #include -#include #include #include #include @@ -49,24 +48,29 @@ namespace moveit { namespace task_constructor { namespace stages { -SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) +SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name) +{ + auto &props = properties(); + props.declare("pregrasp", "pregrasp posture"); + props.declare("grasp", "grasp posture"); +} + +void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - 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" }; + generator_ = generator.get(); + auto ik = new ComputeIK("compute ik", std::move(generator)); + const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object" }; ik->exposePropertiesOfChild(0, grasp_prop_names); - insert(std::move(ik), insertion_position); + insert(std::unique_ptr(ik), insertion_position); exposePropertiesOfChild(insertion_position, grasp_prop_names); exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { - auto allow_touch = std::make_unique(forward ? "allow object collision" : "forbid object collision"); + auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision"); PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); @@ -79,25 +83,25 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) ->getLinkModelNamesWithCollisionGeometry(), true); }); - insert(std::move(allow_touch), insertion_position); + insert(std::unique_ptr(allow_touch), insertion_position); } { auto pipeline = std::make_shared(); pipeline->setTimeout(8.0); pipeline->setPlannerId("RRTConnectkConfigDefault"); - auto move = std::make_unique(forward ? "close gripper" : "open gripper", pipeline); + auto move = new MoveTo(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), insertion_position); - exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp"); + insert(std::unique_ptr(move), insertion_position); + exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp"); } { - auto attach = std::make_unique(forward ? "attach object" : "detach object"); + auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object"); PropertyMap& p = attach->properties(); p.declare("eef"); p.declare("object"); @@ -111,7 +115,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); }); - insert(std::move(attach), insertion_position); + insert(std::unique_ptr(attach), insertion_position); } } @@ -123,7 +127,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) void SimpleGraspBase::setMonitoredStage(Stage* monitored) { - grasp_generator_->setMonitoredStage(monitored); + generator_->setMonitoredStage(monitored); } void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { @@ -133,4 +137,15 @@ void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& setIKFrame(pose_msg); } +SimpleGrasp::SimpleGrasp(std::unique_ptr&& generator, const std::string& name) + : SimpleGraspBase(name) +{ + setup(std::move(generator), true); +} + +SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name) + : SimpleGraspBase(name) { + setup(std::move(generator), false); +} + } } }