ur5 example: use properties

This commit is contained in:
Robert Haschke 2017-12-04 10:26:34 +01:00
parent 7d8f8dfc8d
commit 587dcaebb2
9 changed files with 165 additions and 151 deletions

View File

@ -39,29 +39,32 @@ int main(int argc, char** argv){
spawnObject();
Task t;
// define global properties used by most stages
t.setProperty("group", std::string("arm"));
t.setProperty("eef", std::string("gripper"));
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
t.setProperty("link", std::string("s_model_tool0"));
t.add(std::make_unique<stages::CurrentState>("current state"));
{
auto move = std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("gripper");
move->properties().initFrom(PARENT);
move->setTo("open");
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::Move>("move to pre-grasp");
move->setGroup("arm");
move->setPlannerId("RRTConnectkConfigDefault");
move->properties().initFrom(PARENT);
move->setTimeout(8.0);
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::ref(t.introspection()));
move->setGroup("arm");
move->setLink("s_model_tool0");
// move->addSolutionCallback(std::ref(t.introspection()));
move->properties().initFrom(PARENT);
move->setMinMaxDistance(.03, 0.1);
move->setCartesianStepSize(0.02);
@ -73,8 +76,7 @@ int main(int argc, char** argv){
{
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("gripper");
//gengrasp->setGroup("arm");
gengrasp->properties().initFrom(PARENT);
gengrasp->setGripperGraspPose("open");
gengrasp->setObject("object");
gengrasp->setGraspOffset(.03);
@ -85,7 +87,7 @@ int main(int argc, char** argv){
{
auto move = std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("gripper");
move->properties().initFrom(PARENT);
move->setTo("closed");
move->graspObject("object");
t.add(std::move(move));
@ -93,8 +95,7 @@ int main(int argc, char** argv){
{
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
move->setGroup("arm");
move->setLink("s_model_tool0");
move->properties().initFrom(PARENT);
move->setMinMaxDistance(0.03, 0.05);
move->setCartesianStepSize(0.01);

View File

@ -34,22 +34,10 @@ public:
void setCartesianStepSize(double distance);
protected:
std::string group_;
std::string link_;
double min_distance_;
double max_distance_;
enum {
MODE_ALONG= 1,
MODE_TOWARDS= 2
} mode_;
geometry_msgs::PointStamped towards_;
geometry_msgs::Vector3Stamped along_;
double step_size_;
};
} } }

View File

@ -32,37 +32,15 @@ public:
void setMaxIKSolutions(uint32_t n);
void ignoreCollisions(bool flag);
void setIgnoreCollisions(bool flag);
protected:
planning_scene::PlanningSceneConstPtr scene_;
std::string eef_;
std::string group_;
std::string ik_link_;
double grasp_offset_ = 0.0;
uint32_t max_ik_solutions_;
bool ignore_collisions_ = false;
std::string gripper_grasp_pose_;
std::string object_;
double timeout_ = 0.1;
double angle_delta_ = 0.1;
/* temp values */
double current_angle_ = 0.0;
double remaining_time_;
bool tried_current_state_as_seed_ = false;
std::vector< std::vector<double> > previous_solutions_;

View File

@ -31,11 +31,6 @@ protected:
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
protected:
std::string eef_;
std::string named_target_;
std::string grasp_object_;
std::string attach_link_;
planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
};

View File

@ -4,10 +4,6 @@
#include <moveit/task_constructor/stage.h>
namespace moveit {
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
}
namespace moveit { namespace task_constructor { namespace stages {
class Move : public Connecting {
@ -17,17 +13,12 @@ public:
void init(const planning_scene::PlanningSceneConstPtr &scene);
bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(std::string group);
void setPlannerId(std::string planner);
void setGroup(const std::__cxx11::string &group);
void setPlannerId(const std::__cxx11::string &planner);
void setTimeout(double timeout);
protected:
std::string group_;
std::string planner_id_;
double timeout_;
planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
};
} } }

View File

@ -13,44 +13,51 @@
namespace moveit { namespace task_constructor { namespace stages {
CartesianPositionMotion::CartesianPositionMotion(std::string name)
: PropagatingEitherWay(name),
step_size_(0.005)
: PropagatingEitherWay(name)
{
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "name of link used for IK");
p.declare<double>("min_distance", "minimum distance to move");
p.declare<double>("max_distance", "maximum distance to move");
p.declare<double>("step_size", 0.005);
p.declare<geometry_msgs::PointStamped>("towards", "target point of motion");
p.declare<geometry_msgs::Vector3Stamped>("along", "vector along which to move");
}
void CartesianPositionMotion::setGroup(std::string group){
group_= group;
setProperty("group", group);
}
void CartesianPositionMotion::setLink(std::string link){
link_= link;
setProperty("link", link);
}
void CartesianPositionMotion::setMinDistance(double distance){
min_distance_= distance;
setProperty("min_distance", distance);
}
void CartesianPositionMotion::setMaxDistance(double distance){
max_distance_= distance;
setProperty("max_distance", distance);
}
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
setMinDistance(min_distance);
setMaxDistance(max_distance);
setProperty("min_distance", min_distance);
setProperty("max_distance", max_distance);
}
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
mode_= CartesianPositionMotion::MODE_TOWARDS;
towards_= towards;
setProperty("towards", towards);
}
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
mode_= CartesianPositionMotion::MODE_ALONG;
along_= along;
setProperty("along", along);
}
void CartesianPositionMotion::setCartesianStepSize(double distance){
step_size_= distance;
setProperty("step_size", distance);
}
namespace {
@ -68,8 +75,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
const auto& props = properties();
const std::string& group = props.get<std::string>("group");
const std::string& link = props.get<std::string>("link");
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind(
@ -83,12 +94,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
bool succeeded= false;
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards_.header.frame_id);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
const geometry_msgs::PointStamped& towards = props.get<geometry_msgs::PointStamped>("towards");
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards.header.frame_id);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
Eigen::Vector3d target_point;
tf::pointMsgToEigen(towards_.point, target_point);
tf::pointMsgToEigen(towards.point, target_point);
target_point= frame*target_point;
// retain orientation of link
@ -101,7 +112,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
link_model,
target,
true, /* global frame */
step_size_, /* cartesian step size */
props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */
is_valid);
@ -109,12 +120,13 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
succeeded= achieved_distance >= min_distance_;
succeeded= achieved_distance >= props.get<double>("min_distance");
}
else if( mode_ == CartesianPositionMotion::MODE_ALONG ){
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
Eigen::Vector3d direction;
tf::vectorMsgToEigen(along_.vector, direction);
tf::vectorMsgToEigen(along.vector, direction);
direction= frame.linear()*direction;
double achieved_distance= robot_state.computeCartesianPath(
@ -123,14 +135,14 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
link_model,
direction,
true, /* global frame */
max_distance_, /* distance */
step_size_, /* cartesian step size */
props.get<double>("max_distance"),
props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */
is_valid);
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
succeeded= achieved_distance >= min_distance_;
succeeded= achieved_distance >= props.get<double>("min_distance");
}
else
throw std::runtime_error("position motion has neither a goal nor a direction");
@ -151,8 +163,12 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
planning_scene::PlanningScenePtr result_scene = to.scene()->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
const auto& props = properties();
const std::string& group = props.get<std::string>("group");
const std::string& link = props.get<std::string>("link");
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind(
@ -167,14 +183,15 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
switch(mode_){
case(CartesianPositionMotion::MODE_TOWARDS):
{
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
}
break;
case(CartesianPositionMotion::MODE_ALONG):
{
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
tf::vectorMsgToEigen(along_.vector, direction);
const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
tf::vectorMsgToEigen(along.vector, direction);
direction= frame.linear()*direction;
}
break;
@ -190,14 +207,14 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
link_model,
direction,
true, /* global frame */
max_distance_, /* distance */
step_size_, /* cartesian step size */
props.get<double>("max_distance"),
props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */
is_valid);
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
bool succeeded= achieved_distance >= min_distance_;
bool succeeded= achieved_distance >= props.get<double>("min_distance");
if(succeeded){
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);

View File

@ -21,6 +21,17 @@ namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(name)
{
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("link", "", "name of link used for IK");
p.declare<std::string>("object");
p.declare<std::string>("grasp_pose");
p.declare<double>("timeout", 0.1);
p.declare<uint32_t>("max_ik_solutions", 1);
p.declare<double>("grasp_offset", 0.0);
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
p.declare<bool>("ignore_collisions", false);
}
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -30,40 +41,44 @@ void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
}
void GenerateGraspPose::setGroup(std::string group){
group_= group;
setProperty("group", group);
}
void GenerateGraspPose::setLink(std::string ik_link){
ik_link_= ik_link;
setProperty("link", ik_link);
}
void GenerateGraspPose::setEndEffector(std::string eef){
eef_= eef;
setProperty("eef", eef);
}
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
gripper_grasp_pose_= pose_name;
setProperty("grasp_pose", pose_name);
}
void GenerateGraspPose::setObject(std::string object){
object_= object;
setProperty("object", object);
}
void GenerateGraspPose::setGraspOffset(double offset){
grasp_offset_= offset;
setProperty("grasp_offset", offset);
}
void GenerateGraspPose::setTimeout(double timeout){
timeout_= timeout;
remaining_time_= timeout;
setProperty("timeout", timeout);
}
void GenerateGraspPose::setAngleDelta(double delta){
angle_delta_= delta;
setProperty("angle_delta", delta);
}
void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
max_ik_solutions_= n;
setProperty("max_ik_solutions", n);
}
void GenerateGraspPose::setIgnoreCollisions(bool flag)
{
setProperty("ignore_collisions", flag);
}
bool GenerateGraspPose::canCompute() const{
@ -98,48 +113,58 @@ namespace {
}
bool GenerateGraspPose::compute(){
assert(scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
const auto& props = properties();
double remaining_time = props.get<double>("timeout");
const std::string& eef = props.get<std::string>("eef");
const std::string& group = props.get<std::string>("group");
assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf");
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef);
const moveit::core::JointModelGroup* jmg_active= group_.empty()
const moveit::core::JointModelGroup* jmg_active= group.empty()
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
: grasp_state.getJointModelGroup(group_);
: grasp_state.getJointModelGroup(group);
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
std::string link = props.get<std::string>("link");
if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second;
if(!gripper_grasp_pose_.empty()){
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_);
const std::string& grasp_pose_name = props.get<std::string>("grasp_pose");
if(!grasp_pose_name.empty()){
grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name);
}
const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind(
&isValid,
scene_,
ignore_collisions_,
props.get<bool>("ignore_collisions"),
&previous_solutions_,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3);
geometry_msgs::Pose object_pose, grasp_pose;
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get<std::string>("object"));
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
throw std::runtime_error("requested object does not exist or could not be retrieved");
tf::poseEigenToMsg(object_pose_eigen, object_pose);
double grasp_offset = props.get<double>("grasp_offset");
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
while( canCompute() ){
if( remaining_time_ <= 0.0 || (max_ik_solutions_ != 0 && previous_solutions_.size() >= max_ik_solutions_)){
if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){
std::cout << "computed angle " << current_angle_
<< " with " << previous_solutions_.size()
<< " cached ik solutions"
<< " and " << remaining_time_ << "s left" << std::endl;
current_angle_+= angle_delta_;
remaining_time_= timeout_;
<< " and " << remaining_time << "s left" << std::endl;
current_angle_+= props.get<double>("angle_delta");
remaining_time = props.get<double>("timeout");
tried_current_state_as_seed_= false;
previous_solutions_.clear();
continue;
@ -147,8 +172,8 @@ bool GenerateGraspPose::compute(){
grasp_pose= object_pose;
grasp_pose.position.x-= grasp_offset_*cos(current_angle_);
grasp_pose.position.y-= grasp_offset_*sin(current_angle_);
grasp_pose.position.x-= grasp_offset*cos(current_angle_);
grasp_pose.position.y-= grasp_offset*sin(current_angle_);
grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_);
if(tried_current_state_as_seed_)
@ -156,8 +181,8 @@ bool GenerateGraspPose::compute(){
tried_current_state_as_seed_= true;
auto now= std::chrono::steady_clock::now();
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid);
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, link, 1, remaining_time, is_valid);
remaining_time-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
if(succeeded) {
previous_solutions_.emplace_back();

View File

@ -15,7 +15,13 @@ namespace moveit { namespace task_constructor { namespace stages {
Gripper::Gripper(std::string name)
: PropagatingEitherWay(name)
{}
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("link", "", "name of link the eef is attached to");
p.declare<std::string>("named_target", "", "named target in eef group");
p.declare<std::string>("grasp_object", "", "name of grasp object");
}
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
{
@ -24,25 +30,25 @@ void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
}
void Gripper::setEndEffector(std::string eef){
eef_= eef;
setProperty("eef", eef);
}
void Gripper::setAttachLink(std::string link){
attach_link_= link;
setProperty("link", link);
}
void Gripper::setFrom(std::string named_target){
restrictDirection(BACKWARD);
named_target_= named_target;
setProperty("named_target", named_target);
}
void Gripper::setTo(std::string named_target){
restrictDirection(FORWARD);
named_target_= named_target;
setProperty("named_target", named_target);
}
void Gripper::graspObject(std::string grasp_object){
grasp_object_= grasp_object;
setProperty("grasp_object", grasp_object);
}
bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
@ -50,24 +56,30 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
scene = state.scene()->diff();
assert(scene->getRobotModel());
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
std::string link = props.get<std::string>("link");
const std::string& named_target = props.get<std::string>("named_target");
const std::string& grasp_object = props.get<std::string>("grasp_object");
if(!mgi_){
assert(scene->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf");
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef_);
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef);
const std::string group_name= jmg->getName();
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
if( attach_link_.empty() ){
attach_link_= jmg->getEndEffectorParentGroup().second;
if( link.empty() ){
link= jmg->getEndEffectorParentGroup().second;
}
}
mgi_->setNamedTarget(named_target_);
mgi_->setNamedTarget(named_target);
::planning_interface::MotionPlanRequest req;
mgi_->constructMotionPlanRequest(req);
if( !grasp_object_.empty() ){
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_, mgi_->getLinkNames(), true);
if( !grasp_object.empty() ){
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true);
}
::planning_interface::MotionPlanResponse res;
@ -80,10 +92,10 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
scene->setCurrentState(trajectory->getLastWayPoint());
// attach object
if( !grasp_object_.empty() ){
if( !grasp_object.empty() ){
moveit_msgs::AttachedCollisionObject obj;
obj.link_name= attach_link_;
obj.object.id= grasp_object_;
obj.link_name= link;
obj.object.id= grasp_object;
scene->processAttachedCollisionObjectMsg(obj);
}

View File

@ -14,9 +14,13 @@
namespace moveit { namespace task_constructor { namespace stages {
Move::Move(std::string name)
: Connecting(name),
timeout_(5.0)
{}
: Connecting(name)
{
auto& p = properties();
p.declare<double>("timeout", 5.0, "planning timeout");
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("planner", "", "planner id");
}
void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
{
@ -24,27 +28,30 @@ void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
planner_ = Task::createPlanner(scene->getRobotModel());
}
void Move::setGroup(std::string group){
group_= group;
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
void Move::setGroup(const std::string& group){
setProperty("group", group);
}
void Move::setPlannerId(std::string planner){
planner_id_= planner;
void Move::setPlannerId(const std::string& planner){
setProperty("planner", planner);
}
void Move::setTimeout(double timeout){
timeout_= timeout;
setProperty("timeout", timeout);
}
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
mgi_->setJointValueTarget(to.scene()->getCurrentState());
if( !planner_id_.empty() )
mgi_->setPlannerId(planner_id_);
mgi_->setPlanningTime(timeout_);
const auto& props = properties();
moveit::planning_interface::MoveGroupInterface mgi(props.get<std::string>("group"));
mgi.setJointValueTarget(to.scene()->getCurrentState());
const std::string planner_id = props.get<std::string>("planner");
if( !planner_id.empty() )
mgi.setPlannerId(planner_id);
mgi.setPlanningTime(props.get<double>("timeout"));
::planning_interface::MotionPlanRequest req;
mgi_->constructMotionPlanRequest(req);
mgi.constructMotionPlanRequest(req);
ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res;