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 {
|
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);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user