mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
moved "attach object" from "pick" to "grasp" stage
This commit is contained in:
parent
bff6cc569c
commit
ea6cc4b6bf
@ -56,23 +56,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
|
||||
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
|
||||
insert(std::move(grasp_stage));
|
||||
|
||||
{
|
||||
auto attach = std::make_unique<ModifyPlanningScene>("attach 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){
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||
obj.object.id = p.get<std::string>("object");
|
||||
scene->processAttachedCollisionObjectMsg(obj);
|
||||
});
|
||||
insert(std::move(attach));
|
||||
}
|
||||
|
||||
{
|
||||
auto lift = std::make_unique<MoveRelative>("lift object", cartesian);
|
||||
PropertyMap& p = lift->properties();
|
||||
|
||||
@ -86,7 +86,6 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
|
||||
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
|
||||
move->restrictDirection(MoveTo::FORWARD);
|
||||
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");
|
||||
@ -96,6 +95,22 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
|
||||
insert(std::move(move));
|
||||
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
|
||||
}
|
||||
{
|
||||
auto attach = std::make_unique<ModifyPlanningScene>("attach 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){
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||
obj.object.id = p.get<std::string>("object");
|
||||
scene->processAttachedCollisionObjectMsg(obj);
|
||||
});
|
||||
insert(std::move(attach));
|
||||
}
|
||||
}
|
||||
|
||||
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user