wrap_python_robot_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
42 #include <moveit_msgs/RobotState.h>
43 #include <visualization_msgs/MarkerArray.h>
44 
45 #include <stdexcept>
46 #include <boost/python.hpp>
47 #include <Python.h>
48 
51 namespace bp = boost::python;
52 
53 namespace moveit
54 {
55 class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer
56 {
57 public:
58  RobotInterfacePython(const std::string& robot_description, const std::string& ns = "")
59  : py_bindings_tools::ROScppInitializer()
60  {
61  robot_model_ = planning_interface::getSharedRobotModel(robot_description);
62  if (!robot_model_)
63  throw std::runtime_error("RobotInterfacePython: invalid robot model");
64  current_state_monitor_ =
66  }
67 
68  const char* getRobotName() const
69  {
70  return robot_model_->getName().c_str();
71  }
72 
73  bp::list getJointNames() const
74  {
75  return py_bindings_tools::listFromString(robot_model_->getJointModelNames());
76  }
77 
78  bp::list getGroupJointNames(const std::string& group) const
79  {
80  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
81  if (jmg)
82  return py_bindings_tools::listFromString(jmg->getJointModelNames());
83  else
84  return bp::list();
85  }
86 
87  bp::list getGroupJointTips(const std::string& group) const
88  {
89  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
90  if (jmg)
91  {
92  std::vector<std::string> tips;
93  jmg->getEndEffectorTips(tips);
95  }
96  else
97  return bp::list();
98  }
99 
100  bp::list getLinkNames() const
101  {
102  return py_bindings_tools::listFromString(robot_model_->getLinkModelNames());
103  }
104 
105  bp::list getGroupLinkNames(const std::string& group) const
106  {
107  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
108  if (jmg)
109  return py_bindings_tools::listFromString(jmg->getLinkModelNames());
110  else
111  return bp::list();
112  }
113 
114  bp::list getGroupNames() const
115  {
116  return py_bindings_tools::listFromString(robot_model_->getJointModelGroupNames());
117  }
118 
119  bp::list getJointLimits(const std::string& name) const
120  {
121  bp::list result;
122  const robot_model::JointModel* jm = robot_model_->getJointModel(name);
123  if (jm)
124  {
125  const std::vector<moveit_msgs::JointLimits>& lim = jm->getVariableBoundsMsg();
126  for (std::size_t i = 0; i < lim.size(); ++i)
127  {
128  bp::list l;
129  l.append(lim[i].min_position);
130  l.append(lim[i].max_position);
131  result.append(l);
132  }
133  }
134  return result;
135  }
136 
137  const char* getPlanningFrame() const
138  {
139  return robot_model_->getModelFrame().c_str();
140  }
141 
142  bp::list getLinkPose(const std::string& name)
143  {
144  bp::list l;
145  if (!ensureCurrentState())
146  return l;
147  robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
148  const robot_model::LinkModel* lm = state->getLinkModel(name);
149  if (lm)
150  {
151  const Eigen::Affine3d& t = state->getGlobalLinkTransform(lm);
152  std::vector<double> v(7);
153  v[0] = t.translation().x();
154  v[1] = t.translation().y();
155  v[2] = t.translation().z();
156  Eigen::Quaterniond q(t.linear());
157  v[3] = q.x();
158  v[4] = q.y();
159  v[5] = q.z();
160  v[6] = q.w();
162  }
163  return l;
164  }
165 
166  bp::list getDefaultStateNames(const std::string& group)
167  {
168  bp::list l;
169  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
170  if (jmg)
171  {
172  for (auto& known_state : jmg->getDefaultStateNames())
173  {
174  l.append(known_state);
175  }
176  }
177  return l;
178  }
179 
180  bp::list getCurrentJointValues(const std::string& name)
181  {
182  bp::list l;
183  if (!ensureCurrentState())
184  return l;
185  robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
186  const robot_model::JointModel* jm = state->getJointModel(name);
187  if (jm)
188  {
189  const double* pos = state->getJointPositions(jm);
190  const unsigned int sz = jm->getVariableCount();
191  for (unsigned int i = 0; i < sz; ++i)
192  l.append(pos[i]);
193  }
194 
195  return l;
196  }
197 
198  bp::dict getJointValues(const std::string& group, const std::string& named_state)
199  {
200  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
201  if (!jmg)
202  return boost::python::dict();
203  std::map<std::string, double> values;
204  jmg->getVariableDefaultPositions(named_state, values);
205  return py_bindings_tools::dictFromType(values);
206  }
207 
208  bool ensureCurrentState(double wait = 1.0)
209  {
210  if (!current_state_monitor_)
211  {
212  ROS_ERROR("Unable to get current robot state");
213  return false;
214  }
215 
216  // if needed, start the monitor and wait up to 1 second for a full robot state
217  if (!current_state_monitor_->isActive())
218  {
219  current_state_monitor_->startStateMonitor();
220  if (!current_state_monitor_->waitForCompleteState(wait))
221  ROS_WARN("Joint values for monitored state are requested but the full state is not known");
222  }
223  return true;
224  }
225 
226  std::string getCurrentState()
227  {
228  if (!ensureCurrentState())
229  return "";
230  robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
231  moveit_msgs::RobotState msg;
232  robot_state::robotStateToRobotStateMsg(*s, msg);
234  }
235 
236  bp::tuple getEndEffectorParentGroup(std::string group)
237  {
238  // name of the group that is parent to this end-effector group;
239  // Second: the link this in the parent group that this group attaches to
240  const robot_state::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
241  if (!jmg)
242  return boost::python::make_tuple("", "");
243  std::pair<std::string, std::string> parent_group = jmg->getEndEffectorParentGroup();
244  return boost::python::make_tuple(parent_group.first, parent_group.second);
245  }
246 
247  std::string getRobotMarkersPythonDictList(bp::dict& values, bp::list& links)
248  {
249  robot_state::RobotStatePtr state;
250  if (ensureCurrentState())
251  {
252  state = current_state_monitor_->getCurrentState();
253  }
254  else
255  {
256  state.reset(new robot_state::RobotState(robot_model_));
257  }
258 
259  bp::list k = values.keys();
260  int l = bp::len(k);
261  sensor_msgs::JointState joint_state;
262  joint_state.name.resize(l);
263  joint_state.position.resize(l);
264  for (int i = 0; i < l; ++i)
265  {
266  joint_state.name[i] = bp::extract<std::string>(k[i]);
267  joint_state.position[i] = bp::extract<double>(values[k[i]]);
268  }
269  state->setVariableValues(joint_state);
270  visualization_msgs::MarkerArray msg;
271  state->getRobotMarkers(msg, py_bindings_tools::stringFromList(links));
272 
274  }
275 
276  std::string getRobotMarkersPythonDict(bp::dict& values)
277  {
278  bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames());
279  return getRobotMarkersPythonDictList(values, links);
280  }
281 
282  std::string getRobotMarkersFromMsg(const std::string& state_str)
283  {
284  moveit_msgs::RobotState state_msg;
285  robot_state::RobotState state(robot_model_);
286  py_bindings_tools::deserializeMsg(state_str, state_msg);
287  moveit::core::robotStateMsgToRobotState(state_msg, state);
288 
289  visualization_msgs::MarkerArray msg;
290  state.getRobotMarkers(msg, state.getRobotModel()->getLinkModelNames());
291 
293  }
294 
295  std::string getRobotMarkers()
296  {
297  if (!ensureCurrentState())
298  return "";
299  robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
300  visualization_msgs::MarkerArray msg;
301  s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames());
302 
304  }
305 
306  std::string getRobotMarkersPythonList(bp::list links)
307  {
308  if (!ensureCurrentState())
309  return "";
310  robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
311  visualization_msgs::MarkerArray msg;
312  s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links));
313 
315  }
316 
317  std::string getRobotMarkersGroup(std::string group)
318  {
319  if (!ensureCurrentState())
320  return "";
321  robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
322  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
323  visualization_msgs::MarkerArray msg;
324  if (jmg)
325  {
326  s->getRobotMarkers(msg, jmg->getLinkModelNames());
327  }
328 
330  }
331 
332  std::string getRobotMarkersGroupPythonDict(std::string group, bp::dict& values)
333  {
334  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
335  if (!jmg)
336  return "";
337  bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames());
338  return getRobotMarkersPythonDictList(values, links);
339  }
340 
341  bp::dict getCurrentVariableValues()
342  {
343  bp::dict d;
344 
345  if (!ensureCurrentState())
346  return d;
347 
348  const std::map<std::string, double>& vars = current_state_monitor_->getCurrentStateValues();
349  for (std::map<std::string, double>::const_iterator it = vars.begin(); it != vars.end(); ++it)
350  d[it->first] = it->second;
351 
352  return d;
353  }
354 
355  const char* getRobotRootLink() const
356  {
357  return robot_model_->getRootLinkName().c_str();
358  }
359 
360  bool hasGroup(const std::string& group) const
361  {
362  return robot_model_->hasJointModelGroup(group);
363  }
364 
365 private:
366  robot_model::RobotModelConstPtr robot_model_;
367  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
368  ros::NodeHandle nh_;
369 };
370 }
371 
372 static void wrap_robot_interface()
373 {
374  using namespace moveit;
375 
376  bp::class_<RobotInterfacePython> RobotClass("RobotInterface", bp::init<std::string, bp::optional<std::string>>());
377 
378  RobotClass.def("get_joint_names", &RobotInterfacePython::getJointNames);
379  RobotClass.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames);
380  RobotClass.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames);
381  RobotClass.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips);
382  RobotClass.def("get_group_names", &RobotInterfacePython::getGroupNames);
383  RobotClass.def("get_link_names", &RobotInterfacePython::getLinkNames);
384  RobotClass.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames);
385  RobotClass.def("get_joint_limits", &RobotInterfacePython::getJointLimits);
386  RobotClass.def("get_link_pose", &RobotInterfacePython::getLinkPose);
387  RobotClass.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame);
388  RobotClass.def("get_current_state", &RobotInterfacePython::getCurrentState);
389  RobotClass.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues);
390  RobotClass.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues);
391  RobotClass.def("get_joint_values", &RobotInterfacePython::getJointValues);
392  RobotClass.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink);
393  RobotClass.def("has_group", &RobotInterfacePython::hasGroup);
394  RobotClass.def("get_robot_name", &RobotInterfacePython::getRobotName);
395  RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers);
396  RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList);
397  RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg);
398  RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList);
399  RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict);
400  RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup);
401  RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict);
402  RobotClass.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup);
403 }
404 
405 BOOST_PYTHON_MODULE(_moveit_robot_interface)
406 {
407  wrap_robot_interface();
408 }
409 
d
Definition: setup.py:4
boost::shared_ptr< tf::Transformer > getSharedTF()
q
boost::python::list listFromDouble(const std::vector< double > &v)
boost::python::dict dictFromType(const std::map< std::string, T > &v)
std::vector< double > values
BOOST_PYTHON_MODULE(_moveit_roscpp_initializer)
XmlRpcServer s
std::vector< std::string > stringFromList(const boost::python::object &values)
v
#define ROS_WARN(...)
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr< tf::Transformer > &tf)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelCons...
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
boost::python::list listFromString(const std::vector< std::string > &v)
std::string serializeMsg(const T &msg)
Convert a ROS message to a string.
Definition: serialize_msg.h:48
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
#define ROS_ERROR(...)
void deserializeMsg(const std::string &data, T &msg)
Convert a string to a ROS message.
Definition: serialize_msg.h:66


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:50