From 587dcaebb28d52f6bca98c168d613791b6f58b58 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 Dec 2017 10:26:34 +0100 Subject: [PATCH] ur5 example: use properties --- core/demo/plan_pick_ur5.cpp | 23 +++--- .../stages/cartesian_position_motion.h | 12 --- .../stages/generate_grasp_pose.h | 24 +----- .../moveit/task_constructor/stages/gripper.h | 5 -- .../moveit/task_constructor/stages/move.h | 13 +-- core/src/stages/cartesian_position_motion.cpp | 81 +++++++++++-------- core/src/stages/generate_grasp_pose.cpp | 79 +++++++++++------- core/src/stages/gripper.cpp | 44 ++++++---- core/src/stages/move.cpp | 35 ++++---- 9 files changed, 165 insertions(+), 151 deletions(-) diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 959c5ccc..deb7d624 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -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("current state")); { auto move = std::make_unique("open gripper"); - move->setEndEffector("gripper"); + move->properties().initFrom(PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("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("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("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("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("lift object"); - move->setGroup("arm"); - move->setLink("s_model_tool0"); + move->properties().initFrom(PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h index 0cc48165..93474da6 100644 --- a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h +++ b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h @@ -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_; }; } } } diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 941198ee..2840d33d 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -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 > previous_solutions_; diff --git a/core/include/moveit/task_constructor/stages/gripper.h b/core/include/moveit/task_constructor/stages/gripper.h index 5d57e9d4..8a59e1ef 100644 --- a/core/include/moveit/task_constructor/stages/gripper.h +++ b/core/include/moveit/task_constructor/stages/gripper.h @@ -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_; }; diff --git a/core/include/moveit/task_constructor/stages/move.h b/core/include/moveit/task_constructor/stages/move.h index e7770893..511624cd 100644 --- a/core/include/moveit/task_constructor/stages/move.h +++ b/core/include/moveit/task_constructor/stages/move.h @@ -4,10 +4,6 @@ #include -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_; }; } } } diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index 33c3a5e3..17b49ed4 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -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("group", "name of planning group"); + p.declare("link", "", "name of link used for IK"); + p.declare("min_distance", "minimum distance to move"); + p.declare("max_distance", "maximum distance to move"); + p.declare("step_size", 0.005); + p.declare("towards", "target point of motion"); + p.declare("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("group"); + const std::string& link = props.get("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("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("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("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("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("max_distance"), + props.get("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("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("group"); + const std::string& link = props.get("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("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("max_distance"), + props.get("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("min_distance"); if(succeeded){ robot_trajectory::RobotTrajectoryPtr traj= std::make_shared(robot_state.getRobotModel(), jmg); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 685fd22d..e0cbd68e 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -21,6 +21,17 @@ namespace moveit { namespace task_constructor { namespace stages { GenerateGraspPose::GenerateGraspPose(std::string name) : Generator(name) { + auto& p = properties(); + p.declare("group", "name of planning group"); + p.declare("eef", "name of end-effector group"); + p.declare("link", "", "name of link used for IK"); + p.declare("object"); + p.declare("grasp_pose"); + p.declare("timeout", 0.1); + p.declare("max_ik_solutions", 1); + p.declare("grasp_offset", 0.0); + p.declare("angle_delta", 0.1, "angular steps (rad)"); + p.declare("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("timeout"); + + const std::string& eef = props.get("eef"); + const std::string& group = props.get("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("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("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("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("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("grasp_offset"); + uint32_t max_ik_solutions = props.get("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("angle_delta"); + remaining_time = props.get("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(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(std::chrono::steady_clock::now()- now).count(); if(succeeded) { previous_solutions_.emplace_back(); diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 7903b8c6..331e2485 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -15,7 +15,13 @@ namespace moveit { namespace task_constructor { namespace stages { Gripper::Gripper(std::string name) : PropagatingEitherWay(name) -{} +{ + auto& p = properties(); + p.declare("eef", "name of end-effector group"); + p.declare("link", "", "name of link the eef is attached to"); + p.declare("named_target", "", "named target in eef group"); + p.declare("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("eef"); + std::string link = props.get("link"); + const std::string& named_target = props.get("named_target"); + const std::string& grasp_object = props.get("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(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); } diff --git a/core/src/stages/move.cpp b/core/src/stages/move.cpp index 1284c288..12186720 100644 --- a/core/src/stages/move.cpp +++ b/core/src/stages/move.cpp @@ -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("timeout", 5.0, "planning timeout"); + p.declare("group", "name of planning group"); + p.declare("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(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("group")); + mgi.setJointValueTarget(to.scene()->getCurrentState()); + + const std::string planner_id = props.get("planner"); + if( !planner_id.empty() ) + mgi.setPlannerId(planner_id); + mgi.setPlanningTime(props.get("timeout")); ::planning_interface::MotionPlanRequest req; - mgi_->constructMotionPlanRequest(req); + mgi.constructMotionPlanRequest(req); ros::Duration(4.0).sleep(); ::planning_interface::MotionPlanResponse res;