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 {
/** 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);
}
};
} } }

View File

@ -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<std::string>("eef_parent_group", "JMG of eef's parent");
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();
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<MoveRelative>("lift object", cartesian_solver_);
auto lift = std::make_unique<MoveRelative>(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<std::string>("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_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);