From 2c08d9080dae6036e28e982098d012fa466c588f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 01:16:36 +0100 Subject: [PATCH] Pick, Place as specializations of PickPlaceBase --- .../moveit/task_constructor/stages/pick.h | 59 ++++++++++++++++--- core/src/stages/pick.cpp | 25 ++++---- 2 files changed, 65 insertions(+), 19 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index a12c65f3..10c2c8db 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -48,26 +48,30 @@ MOVEIT_CLASS_FORWARD(CartesianPath) namespace stages { -/** Pick wraps a complete pipeline to pick up an object with a given end effector. +/** PickPlaceBase wraps the pipeline to pick or place 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 * + * Placing consist of the inverse order of stages: + * - place down along a given direction + * - detach the object + * - linearly retract end effector + * * 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 { +class PickPlaceBase : public SerialContainer { solvers::CartesianPathPtr cartesian_solver_; Stage* grasp_stage_ = nullptr; Stage* approach_stage_ = nullptr; Stage* lift_stage_ = nullptr; public: - Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick"); + PickPlaceBase(Stage::pointer &&grasp_stage, const std::string& name, bool forward); void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -80,10 +84,49 @@ public: solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } - 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); + void setApproachRetract(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); + void setLiftPlace(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); }; + +/// specialization of PickPlaceBase to realize picking +class Pick : public PickPlaceBase { +public: + Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick") + : PickPlaceBase(std::move(grasp_stage), name, true) + {} + + void setApproachMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setApproachRetract(motion, min_distance, max_distance); + } + + void setLiftMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setLiftPlace(motion, min_distance, max_distance); + } +}; + + +/// specialization of PickPlaceBase to realize placing +class Place : public PickPlaceBase { +public: + Place(Stage::pointer &&ungrasp_stage, const std::string& name = "place") + : PickPlaceBase(std::move(ungrasp_stage), name, false) + {} + + void setRetractMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setApproachRetract(motion, min_distance, max_distance); + } + + void setPlaceMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setLiftPlace(motion, min_distance, max_distance); + } +}; + + } } } diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 50504889..b098749f 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -9,7 +9,7 @@ namespace moveit { namespace task_constructor { namespace stages { -Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) +PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward) : SerialContainer(name) { PropertyMap& p = properties(); @@ -22,33 +22,34 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) p.declare("eef_parent_group", "JMG of eef's parent"); cartesian_solver_ = std::make_shared(); + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - auto approach = std::make_unique("approach object", cartesian_solver_); + 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.set("marker_ns", std::string("approach")); + p.set("marker_ns", std::string(forward ? "approach" : "retract")); approach_stage_ = approach.get(); - insert(std::move(approach)); + insert(std::move(approach), insertion_position); } grasp_stage_ = grasp_stage.get(); grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); - insert(std::move(grasp_stage)); + insert(std::move(grasp_stage), insertion_position); { - auto lift = std::make_unique("lift object", cartesian_solver_); + 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.set("marker_ns", std::string("lift")); + p.set("marker_ns", std::string(forward ? "lift" : "place")); lift_stage_ = lift.get(); - insert(std::move(lift)); + insert(std::move(lift), insertion_position); } } -void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) +void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { // inherit properties from parent PropertyMap* p = &properties(); @@ -57,6 +58,8 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) // init internal properties const std::string &eef = p->get("eef"); const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef); + if (!jmg) throw InitStageException(*this, "unknown end effector: " + eef); + p->set("eef_group", jmg->getName()); p->set("eef_parent_group", jmg->getEndEffectorParentGroup().first); @@ -64,7 +67,7 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) SerialContainer::init(robot_model); } -void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = approach_stage_->properties(); p.set("twist", motion); @@ -72,7 +75,7 @@ void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double m p.set("max_distance", max_distance); } -void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = lift_stage_->properties(); p.set("twist", motion);