pick: reusable stage for picking up an object

This commit is contained in:
Robert Haschke 2018-02-16 23:36:11 +01:00
parent d731e943f1
commit bff6cc569c
11 changed files with 461 additions and 84 deletions

View File

@ -105,7 +105,7 @@ int main(int argc, char** argv){
{
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->properties().configureInitFrom(Stage::PARENT);
gengrasp->setGripperGraspPose("open");
gengrasp->setNamedPose("open");
gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),

View File

@ -1,11 +1,8 @@
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/gripper.h>
#include <moveit/task_constructor/stages/move.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/stages/pick.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
@ -44,78 +41,31 @@ int main(int argc, char** argv){
Task t;
Stage* initial_stage = nullptr;
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
{
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
}
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
grasp_generator->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
grasp_generator->setAngleDelta(.2);
grasp_generator->setPreGraspPose("open");
grasp_generator->setGraspPose("closed");
grasp_generator->setMonitoredStage(initial_stage);
{
auto move= std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("left_gripper");
move->setTo("open");
t.add(std::move(move));
}
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
pick->setProperty("eef", std::string("left_gripper"));
pick->setProperty("object", std::string("object"));
geometry_msgs::TwistStamped approach;
approach.header.frame_id = "l_gripper_tool_frame";
approach.twist.linear.x = 1.0;
pick->setApproachMotion(approach, 0.03, 0.1);
{
auto move= std::make_unique<stages::Move>("move to pre-grasp");
move->setGroup("left_arm");
move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0);
t.add(std::move(move));
}
geometry_msgs::TwistStamped lift;
lift.header.frame_id = "base_link";
lift.twist.linear.z = 1.0;
pick->setLiftMotion(lift, 0.03, 0.05);
{
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(.03, 0.1);
move->setCartesianStepSize(0.02);
geometry_msgs::PointStamped target;
target.header.frame_id= "object";
move->towards(target);
t.add(std::move(move));
}
{
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("left_gripper");
gengrasp->setGripperGraspPose("open");
gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
gengrasp->setAngleDelta(.2);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->setEndEffector("left_gripper");
t.add(std::move(ik));
}
{
auto move= std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("left_gripper");
move->setAttachLink("l_gripper_tool_frame");
move->setTo("closed");
move->graspObject("object");
t.add(std::move(move));
}
{
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(0.03, 0.05);
move->setCartesianStepSize(0.01);
geometry_msgs::Vector3Stamped direction;
direction.header.frame_id= "base_link";
direction.vector.z= 1.0;
move->along(direction);
t.add(std::move(move));
}
t.add(std::move(pick));
try {
t.plan();

View File

@ -84,7 +84,7 @@ int main(int argc, char** argv){
{
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->properties().configureInitFrom(Stage::PARENT);
gengrasp->setGripperGraspPose("open");
gengrasp->setNamedPose("open");
gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
gengrasp->setAngleDelta(-.2);

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Geometry>
namespace moveit { namespace task_constructor { namespace stages {
@ -52,7 +53,7 @@ public:
bool compute() override;
void setEndEffector(const std::string &eef);
void setGripperGraspPose(const std::string &pose_name);
void setNamedPose(const std::string &pose_name);
void setObject(const std::string &object);
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform);

View File

@ -60,7 +60,7 @@ namespace moveit { namespace task_constructor { namespace stages {
class ModifyPlanningScene : public PropagatingEitherWay {
public:
typedef std::vector<std::string> Names;
typedef std::function<void(planning_scene::PlanningScenePtr scene, PropertyMap& properties)> ApplyCallback;
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)> ApplyCallback;
ModifyPlanningScene(const std::string& name = "modify planning scene");
bool computeForward(const InterfaceState& from) override;

View File

@ -0,0 +1,79 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke */
#pragma once
#include <moveit/task_constructor/container.h>
#include <geometry_msgs/TwistStamped.h>
namespace moveit { namespace task_constructor { namespace stages {
/** Pick wraps a complete pipeline to pick up 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
*
* 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 {
Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr;
public:
Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void setEndEffector(const std::string& eef) {
properties().set<std::string>("eef", eef);
}
void setObject(const std::string& object) {
properties().set<std::string>("object", object);
}
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);
};
} } }

View File

@ -0,0 +1,101 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke */
#pragma once
#include <moveit/task_constructor/container.h>
#include <moveit/macros/class_forward.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Geometry>
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose;
/** Simple Grasp Stage
*
* 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.
*/
class SimpleGrasp : public SerialContainer {
moveit::core::RobotModelConstPtr model_;
GenerateGraspPose* grasp_generator_ = nullptr;
public:
SimpleGrasp(const std::string& name = "grasp");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void setMonitoredStage(Stage* monitored);
void setEndEffector(const std::string& eef) {
properties().set<std::string>("eef", eef);
}
void setObject(const std::string& object) {
properties().set<std::string>("object", object);
}
void setPreGraspPose(const std::string& pregrasp) {
properties().set<std::string>("pregrasp", pregrasp);
}
void setGraspPose(const std::string& grasp) {
properties().set<std::string>("grasp", grasp);
}
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) {
properties().set("tool_to_grasp_tf", transform);
}
void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = "");
template <typename T>
void setToolToGraspTF(const T& t, const std::string& link = "") {
Eigen::Affine3d transform; transform = t;
setToolToGraspTF(transform, link);
}
void setAngleDelta(double angle_delta) {
properties().set("angle_delta", angle_delta);
}
void setMaxIKSolutions(uint32_t max_ik_solutions) {
properties().set("max_ik_solutions", max_ik_solutions);
}
void setTimeout(double timeout) {
properties().set("timeout", timeout);
}
};
} } }

View File

@ -11,6 +11,9 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/move_to.h
${PROJECT_INCLUDE}/stages/move_relative.h
${PROJECT_INCLUDE}/stages/simple_grasp.h
${PROJECT_INCLUDE}/stages/pick.h
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
${PROJECT_INCLUDE}/stages/gripper.h
${PROJECT_INCLUDE}/stages/move.h
@ -27,6 +30,9 @@ add_library(${PROJECT_NAME}_stages
move_to.cpp
move_relative.cpp
simple_grasp.cpp
pick.cpp
cartesian_position_motion.cpp
gripper.cpp
move.cpp

View File

@ -50,8 +50,8 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
: MonitoringGenerator(name)
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("eef_named_pose");
p.declare<std::string>("eef", "name of end-effector");
p.declare<std::string>("pregrasp", "name of end-effector's pregrasp pose");
p.declare<std::string>("object");
p.declare<geometry_msgs::TransformStamped>("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame");
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
@ -61,8 +61,8 @@ void GenerateGraspPose::setEndEffector(const std::string &eef){
setProperty("eef", eef);
}
void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){
setProperty("eef_named_pose", pose_name);
void GenerateGraspPose::setNamedPose(const std::string &pose_name){
setProperty("pregrasp", pose_name);
}
void GenerateGraspPose::setObject(const std::string &object){
@ -111,11 +111,13 @@ bool GenerateGraspPose::compute(){
const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef);
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
const std::string& eef_named_pose = props.get<std::string>("eef_named_pose");
robot_state.setToDefaultValues(jmg , eef_named_pose);
const std::string& joint_pose = props.get<std::string>("pregrasp");
if(!joint_pose.empty()){
robot_state.setToDefaultValues(jmg , joint_pose);
}
geometry_msgs::TransformStamped tool2grasp_msg = props.get<geometry_msgs::TransformStamped>("tool_to_grasp_tf");
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
const std::string &link_name = jmg->getEndEffectorParentGroup().second;
if (tool2grasp_msg.header.frame_id.empty()) // if no frame_id is given
tool2grasp_msg.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame
Eigen::Affine3d to_grasp;

118
core/src/stages/pick.cpp Normal file
View File

@ -0,0 +1,118 @@
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
: SerialContainer(name)
{
PropertyMap& p = properties();
p.declare<std::string>("eef", "end effector name");
p.declare<std::string>("object", "name of object to grasp");
// internal properties (cannot be marked as such yet)
p.declare<std::string>("eef_group", "JMG of eef");
p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
auto cartesian = std::make_shared<solvers::CartesianPath>();
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
{
auto move = std::make_unique<MoveTo>("open gripper", pipeline);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_group");
move->setGoal("open"); // TODO: retrieve from grasp stage
insert(std::move(move));
}
{
auto move = std::make_unique<Connect>("move to object", pipeline);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
insert(std::move(move));
}
{
auto approach = std::make_unique<MoveRelative>("approach object", cartesian);
PropertyMap& p = approach->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.set("marker_ns", std::string("approach"));
approach_stage_ = approach.get();
insert(std::move(approach));
}
grasp_stage_ = grasp_stage.get();
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();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.set("marker_ns", std::string("lift"));
lift_stage_ = lift.get();
insert(std::move(lift));
}
}
void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
{
// inherit properties from parent
PropertyMap* p = &properties();
p->performInitFrom(Stage::PARENT, parent()->properties());
// init internal properties
const std::string &eef = p->get<std::string>("eef");
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
p->set<std::string>("eef_group", jmg->getName());
p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first);
// propagate my properties to children (and do standard init)
SerialContainer::init(robot_model);
}
void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
auto& p = approach_stage_->properties();
p.set("twist", motion);
p.set("min_distance", min_distance);
p.set("max_distance", max_distance);
}
void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
auto& p = lift_stage_->properties();
p.set("twist", motion);
p.set("min_distance", min_distance);
p.set("max_distance", max_distance);
}
} } }

View File

@ -0,0 +1,120 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke */
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/planning_scene/planning_scene.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
SimpleGrasp::SimpleGrasp(const std::string& name)
: SerialContainer(name)
{
{
auto gengrasp = std::make_unique<GenerateGraspPose>("generate grasp 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", "tool_to_grasp_tf" };
ik->exposePropertiesOfChild(0, grasp_prop_names);
insert(std::move(ik));
exposePropertiesOfChild(-1, grasp_prop_names);
exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" });
}
{
auto allow_touch = std::make_unique<ModifyPlanningScene>("enable object collision");
PropertyMap& p = allow_touch->properties();
p.declare<std::string>("eef");
p.declare<std::string>("object");
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
const std::string& eef = p.get<std::string>("eef");
const std::string& object = p.get<std::string>("object");
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
->getLinkModelNamesWithCollisionGeometry(), true);
});
insert(std::move(allow_touch));
}
{
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
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");
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
return boost::any(jmg->getName());
});
insert(std::move(move));
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
}
}
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
{
model_ = robot_model;
SerialContainer::init(robot_model);
}
void SimpleGrasp::setMonitoredStage(Stage* monitored)
{
grasp_generator_->setMonitoredStage(monitored);
}
void SimpleGrasp::setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link) {
geometry_msgs::TransformStamped stamped;
stamped.header.frame_id = link;
stamped.child_frame_id = "grasp_frame";
tf::transformEigenToMsg(transform, stamped.transform);
setToolToGraspTF(stamped);
}
} } }