mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Pick, Place as specializations of PickPlaceBase
This commit is contained in:
parent
ce5d7c63f0
commit
2c08d9080d
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
} } }
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user