mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
SimpleGrasp, SimpleUnGrasp as specializations of SimpleGraspBase
This commit is contained in:
parent
97d2eb5c6a
commit
ce5d7c63f0
@ -46,18 +46,20 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class GenerateGraspPose;
|
||||
|
||||
/** Simple Grasp Stage
|
||||
/** base class for simple grasping / ungrasping
|
||||
*
|
||||
* Given a named pre-grasp and grasp posture the stage generates
|
||||
* fully-qualified pre-grasp and grasp robot states, connected
|
||||
* by a grasping trajectory of the end-effector.
|
||||
* Given a named pre-grasp and grasp posture the stage generates fully-qualified
|
||||
* pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector.
|
||||
*
|
||||
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
|
||||
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
|
||||
*/
|
||||
class SimpleGrasp : public SerialContainer {
|
||||
class SimpleGraspBase : public SerialContainer {
|
||||
moveit::core::RobotModelConstPtr model_;
|
||||
GenerateGraspPose* grasp_generator_ = nullptr;
|
||||
|
||||
public:
|
||||
SimpleGrasp(const std::string& name = "grasp");
|
||||
SimpleGraspBase(const std::string& name, bool forward);
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
void setMonitoredStage(Stage* monitored);
|
||||
@ -101,4 +103,18 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/// specialization of SimpleGraspBase to realize grasping
|
||||
class SimpleGrasp : public SimpleGraspBase {
|
||||
public:
|
||||
SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {}
|
||||
};
|
||||
|
||||
|
||||
/// specialization of SimpleGraspBase to realize ungrasping
|
||||
class SimpleUnGrasp : public SimpleGraspBase {
|
||||
public:
|
||||
SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {}
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -49,23 +49,24 @@
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
SimpleGrasp::SimpleGrasp(const std::string& name)
|
||||
SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
||||
: SerialContainer(name)
|
||||
{
|
||||
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
||||
{
|
||||
auto gengrasp = std::make_unique<GenerateGraspPose>("generate grasp pose");
|
||||
auto gengrasp = std::make_unique<GenerateGraspPose>(forward ? "generate grasp pose" : "generate release pose");
|
||||
grasp_generator_ = gengrasp.get();
|
||||
|
||||
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
|
||||
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" };
|
||||
ik->exposePropertiesOfChild(0, grasp_prop_names);
|
||||
insert(std::move(ik));
|
||||
insert(std::move(ik), insertion_position);
|
||||
|
||||
exposePropertiesOfChild(-1, grasp_prop_names);
|
||||
exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout", "ik_frame" });
|
||||
exposePropertiesOfChild(insertion_position, grasp_prop_names);
|
||||
exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" });
|
||||
}
|
||||
{
|
||||
auto allow_touch = std::make_unique<ModifyPlanningScene>("allow object collision");
|
||||
auto allow_touch = std::make_unique<ModifyPlanningScene>(forward ? "allow object collision" : "forbid object collision");
|
||||
PropertyMap& p = allow_touch->properties();
|
||||
p.declare<std::string>("eef");
|
||||
p.declare<std::string>("object");
|
||||
@ -78,53 +79,54 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
|
||||
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
||||
->getLinkModelNamesWithCollisionGeometry(), true);
|
||||
});
|
||||
insert(std::move(allow_touch));
|
||||
insert(std::move(allow_touch), insertion_position);
|
||||
}
|
||||
{
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||
pipeline->setTimeout(8.0);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
|
||||
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
|
||||
auto move = std::make_unique<MoveTo>(forward ? "close gripper" : "open gripper", pipeline);
|
||||
PropertyMap& p = move->properties();
|
||||
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
||||
const std::string& eef = parent_map.get<std::string>("eef");
|
||||
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
||||
return boost::any(jmg->getName());
|
||||
});
|
||||
insert(std::move(move));
|
||||
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
|
||||
insert(std::move(move), insertion_position);
|
||||
exposePropertyOfChildAs(insertion_position, "joint_pose", forward ? "grasp" : "pregrasp");
|
||||
}
|
||||
{
|
||||
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
|
||||
auto attach = std::make_unique<ModifyPlanningScene>(forward ? "attach object" : "detach object");
|
||||
PropertyMap& p = attach->properties();
|
||||
p.declare<std::string>("eef");
|
||||
p.declare<std::string>("object");
|
||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
||||
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||
attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
||||
obj.object.operation = forward ? (int8_t) moveit_msgs::CollisionObject::ADD
|
||||
: (int8_t) moveit_msgs::CollisionObject::REMOVE;
|
||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||
obj.object.id = p.get<std::string>("object");
|
||||
scene->processAttachedCollisionObjectMsg(obj);
|
||||
});
|
||||
insert(std::move(attach));
|
||||
insert(std::move(attach), insertion_position);
|
||||
}
|
||||
}
|
||||
|
||||
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
model_ = robot_model;
|
||||
SerialContainer::init(robot_model);
|
||||
}
|
||||
|
||||
void SimpleGrasp::setMonitoredStage(Stage* monitored)
|
||||
void SimpleGraspBase::setMonitoredStage(Stage* monitored)
|
||||
{
|
||||
grasp_generator_->setMonitoredStage(monitored);
|
||||
}
|
||||
|
||||
void SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
||||
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
||||
geometry_msgs::PoseStamped pose_msg;
|
||||
pose_msg.header.frame_id = link;
|
||||
tf::poseEigenToMsg(pose, pose_msg.pose);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user