From 267c2142888a8e3523e7d780ba950e6fe46947fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 10 May 2019 14:41:46 +0200 Subject: [PATCH] Overload setGoal to accept map of joint values as argument (#87) --- .../moveit/task_constructor/stages/move_to.h | 3 +++ core/src/stages/move_to.cpp | 14 ++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index d19bc52e..0844724b 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -95,6 +95,9 @@ public: setProperty("goal", robot_state); } + /// move joints by name to their mapped target value + void setGoal(const std::map& joints); + void setPathConstraints(moveit_msgs::Constraints path_constraints){ setProperty("path_constraints", std::move(path_constraints)); } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 49891110..17710f11 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -71,6 +71,20 @@ void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) setIKFrame(pose_msg); } +void MoveTo::setGoal(const std::map& joints) { + moveit_msgs::RobotState robot_state; + robot_state.joint_state.name.reserve(joints.size()); + robot_state.joint_state.position.reserve(joints.size()); + + for (auto& joint : joints) + { + robot_state.joint_state.name.push_back(joint.first); + robot_state.joint_state.position.push_back(joint.second); + } + robot_state.is_diff = true; + setProperty("goal", robot_state); +} + void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) { PropagatingEitherWay::init(robot_model);