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 
44 #include <stdexcept>
45 #include <boost/python.hpp>
46 #include <Python.h>
47 
50 namespace bp = boost::python;
51 
52 namespace moveit
53 {
54 class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer
55 {
56 public:
57  RobotInterfacePython(const std::string& robot_description) : py_bindings_tools::ROScppInitializer()
58  {
59  robot_model_ = planning_interface::getSharedRobotModel(robot_description);
60  if (!robot_model_)
61  throw std::runtime_error("RobotInterfacePython: invalid robot model");
62  current_state_monitor_ = planning_interface::getSharedStateMonitor(robot_model_, planning_interface::getSharedTF());
63  }
64 
65  const char* getRobotName() const
66  {
67  return robot_model_->getName().c_str();
68  }
69 
70  bp::list getJointNames() const
71  {
72  return py_bindings_tools::listFromString(robot_model_->getJointModelNames());
73  }
74 
75  bp::list getGroupJointNames(const std::string& group) const
76  {
77  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
78  if (jmg)
79  return py_bindings_tools::listFromString(jmg->getJointModelNames());
80  else
81  return bp::list();
82  }
83 
84  bp::list getGroupJointTips(const std::string& group) const
85  {
86  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
87  if (jmg)
88  {
89  std::vector<std::string> tips;
90  jmg->getEndEffectorTips(tips);
92  }
93  else
94  return bp::list();
95  }
96 
97  bp::list getLinkNames() const
98  {
99  return py_bindings_tools::listFromString(robot_model_->getLinkModelNames());
100  }
101 
102  bp::list getGroupLinkNames(const std::string& group) const
103  {
104  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
105  if (jmg)
106  return py_bindings_tools::listFromString(jmg->getLinkModelNames());
107  else
108  return bp::list();
109  }
110 
111  bp::list getGroupNames() const
112  {
113  return py_bindings_tools::listFromString(robot_model_->getJointModelGroupNames());
114  }
115 
116  bp::list getJointLimits(const std::string& name) const
117  {
118  bp::list result;
119  const robot_model::JointModel* jm = robot_model_->getJointModel(name);
120  if (jm)
121  {
122  const std::vector<moveit_msgs::JointLimits>& lim = jm->getVariableBoundsMsg();
123  for (std::size_t i = 0; i < lim.size(); ++i)
124  {
125  bp::list l;
126  l.append(lim[i].min_position);
127  l.append(lim[i].max_position);
128  result.append(l);
129  }
130  }
131  return result;
132  }
133 
134  const char* getPlanningFrame() const
135  {
136  return robot_model_->getModelFrame().c_str();
137  }
138 
139  bp::list getLinkPose(const std::string& name)
140  {
141  bp::list l;
142  if (!ensureCurrentState())
143  return l;
144  robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
145  const robot_model::LinkModel* lm = state->getLinkModel(name);
146  if (lm)
147  {
148  const Eigen::Affine3d& t = state->getGlobalLinkTransform(lm);
149  std::vector<double> v(7);
150  v[0] = t.translation().x();
151  v[1] = t.translation().y();
152  v[2] = t.translation().z();
153  Eigen::Quaterniond q(t.rotation());
154  v[3] = q.x();
155  v[4] = q.y();
156  v[5] = q.z();
157  v[6] = q.w();
159  }
160  return l;
161  }
162 
163  bp::list getCurrentJointValues(const std::string& name)
164  {
165  bp::list l;
166  if (!ensureCurrentState())
167  return l;
168  robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
169  const robot_model::JointModel* jm = state->getJointModel(name);
170  if (jm)
171  {
172  const double* pos = state->getJointPositions(jm);
173  const unsigned int sz = jm->getVariableCount();
174  for (unsigned int i = 0; i < sz; ++i)
175  l.append(pos[i]);
176  }
177 
178  return l;
179  }
180 
181  bool ensureCurrentState(double wait = 1.0)
182  {
183  if (!current_state_monitor_)
184  {
185  ROS_ERROR("Unable to get current robot state");
186  return false;
187  }
188 
189  // if needed, start the monitor and wait up to 1 second for a full robot state
190  if (!current_state_monitor_->isActive())
191  {
192  current_state_monitor_->startStateMonitor();
193  if (!current_state_monitor_->waitForCompleteState(wait))
194  ROS_WARN("Joint values for monitored state are requested but the full state is not known");
195  }
196  return true;
197  }
198 
199  std::string getCurrentState()
200  {
201  if (!ensureCurrentState())
202  return "";
203  robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
204  moveit_msgs::RobotState msg;
205  robot_state::robotStateToRobotStateMsg(*s, msg);
207  }
208 
209  bp::dict getCurrentVariableValues()
210  {
211  bp::dict d;
212 
213  if (!ensureCurrentState())
214  return d;
215 
216  const std::map<std::string, double>& vars = current_state_monitor_->getCurrentStateValues();
217  for (std::map<std::string, double>::const_iterator it = vars.begin(); it != vars.end(); ++it)
218  d[it->first] = it->second;
219 
220  return d;
221  }
222 
223  const char* getRobotRootLink() const
224  {
225  return robot_model_->getRootLinkName().c_str();
226  }
227 
228  bool hasGroup(const std::string& group) const
229  {
230  return robot_model_->hasJointModelGroup(group);
231  }
232 
233 private:
234  robot_model::RobotModelConstPtr robot_model_;
235  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
236 };
237 }
238 
239 static void wrap_robot_interface()
240 {
241  using namespace moveit;
242 
243  bp::class_<RobotInterfacePython> RobotClass("RobotInterface", bp::init<std::string>());
244 
245  RobotClass.def("get_joint_names", &RobotInterfacePython::getJointNames);
246  RobotClass.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames);
247  RobotClass.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips);
248  RobotClass.def("get_group_names", &RobotInterfacePython::getGroupNames);
249  RobotClass.def("get_link_names", &RobotInterfacePython::getLinkNames);
250  RobotClass.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames);
251  RobotClass.def("get_joint_limits", &RobotInterfacePython::getJointLimits);
252  RobotClass.def("get_link_pose", &RobotInterfacePython::getLinkPose);
253  RobotClass.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame);
254  RobotClass.def("get_current_state", &RobotInterfacePython::getCurrentState);
255  RobotClass.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues);
256  RobotClass.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues);
257  RobotClass.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink);
258  RobotClass.def("has_group", &RobotInterfacePython::hasGroup);
259  RobotClass.def("get_robot_name", &RobotInterfacePython::getRobotName);
260 }
261 
262 BOOST_PYTHON_MODULE(_moveit_robot_interface)
263 {
264  wrap_robot_interface();
265 }
266 
d
Definition: setup.py:4
boost::shared_ptr< tf::Transformer > getSharedTF()
boost::python::list listFromDouble(const std::vector< double > &v)
BOOST_PYTHON_MODULE(_moveit_roscpp_initializer)
XmlRpcServer s
#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...
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(...)


planning_interface
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:52:20