Pick, Place as specializations of PickPlaceBase

This commit is contained in:
Robert Haschke 2018-03-13 01:16:36 +01:00
parent ce5d7c63f0
commit 2c08d9080d
2 changed files with 65 additions and 19 deletions

View File

@ -48,26 +48,30 @@ MOVEIT_CLASS_FORWARD(CartesianPath)
namespace stages { 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: * 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 * - linearly approaching the object along an approach direction/twist
* - "grasp" end effector posture * - "grasp" end effector posture
* - attach object * - attach object
* - lift along along a given direction/twist * - 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 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. * 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_; solvers::CartesianPathPtr cartesian_solver_;
Stage* grasp_stage_ = nullptr; Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr; Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr; Stage* lift_stage_ = nullptr;
public: 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; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@ -80,10 +84,49 @@ public:
solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; }
void setApproachMotion(const geometry_msgs::TwistStamped& motion, void setApproachRetract(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance); double min_distance, double max_distance);
void setLiftMotion(const geometry_msgs::TwistStamped& motion, void setLiftPlace(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance); 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);
}
};
} } } } } }

View File

@ -9,7 +9,7 @@
namespace moveit { namespace task_constructor { namespace stages { 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) : SerialContainer(name)
{ {
PropertyMap& p = properties(); PropertyMap& p = properties();
@ -22,33 +22,34 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
p.declare<std::string>("eef_parent_group", "JMG of eef's parent"); p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
cartesian_solver_ = std::make_shared<solvers::CartesianPath>(); cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
{ {
auto approach = std::make_unique<MoveRelative>("approach object", cartesian_solver_); auto approach = std::make_unique<MoveRelative>(forward ? "approach object" : "retract", cartesian_solver_);
PropertyMap& p = approach->properties(); PropertyMap& p = approach->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); 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(); approach_stage_ = approach.get();
insert(std::move(approach)); insert(std::move(approach), insertion_position);
} }
grasp_stage_ = grasp_stage.get(); grasp_stage_ = grasp_stage.get();
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); 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<MoveRelative>("lift object", cartesian_solver_); auto lift = std::make_unique<MoveRelative>(forward ? "lift object" : "place object", cartesian_solver_);
PropertyMap& p = lift->properties(); PropertyMap& p = lift->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); 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(); 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 // inherit properties from parent
PropertyMap* p = &properties(); PropertyMap* p = &properties();
@ -57,6 +58,8 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
// init internal properties // init internal properties
const std::string &eef = p->get<std::string>("eef"); const std::string &eef = p->get<std::string>("eef");
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef); const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
if (!jmg) throw InitStageException(*this, "unknown end effector: " + eef);
p->set<std::string>("eef_group", jmg->getName()); p->set<std::string>("eef_group", jmg->getName());
p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first); p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first);
@ -64,7 +67,7 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
SerialContainer::init(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(); auto& p = approach_stage_->properties();
p.set("twist", motion); p.set("twist", motion);
@ -72,7 +75,7 @@ void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double m
p.set("max_distance", max_distance); 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(); auto& p = lift_stage_->properties();
p.set("twist", motion); p.set("twist", motion);