wrap_python_move_group.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 
46 
47 #include <boost/python.hpp>
48 #include <boost/shared_ptr.hpp>
49 #include <Python.h>
50 
53 namespace bp = boost::python;
54 
55 namespace moveit
56 {
57 namespace planning_interface
58 {
59 class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public MoveGroupInterface
60 {
61 public:
62  // ROSInitializer is constructed first, and ensures ros::init() was called, if
63  // needed
64  MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description)
65  : py_bindings_tools::ROScppInitializer()
66  , MoveGroupInterface(Options(group_name, robot_description), boost::shared_ptr<tf::Transformer>(),
67  ros::WallDuration(5, 0))
68  {
69  }
70 
71  bool setJointValueTargetPerJointPythonList(const std::string& joint, bp::list& values)
72  {
73  return setJointValueTarget(joint, py_bindings_tools::doubleFromList(values));
74  }
75 
76  bool setJointValueTargetPythonIterable(bp::object& values)
77  {
78  return setJointValueTarget(py_bindings_tools::doubleFromList(values));
79  }
80 
81  bool setJointValueTargetPythonDict(bp::dict& values)
82  {
83  bp::list k = values.keys();
84  int l = bp::len(k);
85  std::map<std::string, double> v;
86  for (int i = 0; i < l; ++i)
87  v[bp::extract<std::string>(k[i])] = bp::extract<double>(values[k[i]]);
88  return setJointValueTarget(v);
89  }
90 
91  bool setJointValueTargetFromPosePython(const std::string& pose_str, const std::string& eef, bool approx)
92  {
93  geometry_msgs::Pose pose_msg;
94  py_bindings_tools::deserializeMsg(pose_str, pose_msg);
95  return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
96  }
97 
98  bool setJointValueTargetFromPoseStampedPython(const std::string& pose_str, const std::string& eef, bool approx)
99  {
100  geometry_msgs::PoseStamped pose_msg;
101  py_bindings_tools::deserializeMsg(pose_str, pose_msg);
102  return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
103  }
104 
105  bool setJointValueTargetFromJointStatePython(const std::string& js_str)
106  {
107  sensor_msgs::JointState js_msg;
108  py_bindings_tools::deserializeMsg(js_str, js_msg);
109  return setJointValueTarget(js_msg);
110  }
111 
112  bp::list getJointValueTargetPythonList()
113  {
114  const robot_state::RobotState& values = moveit::planning_interface::MoveGroupInterface::getJointValueTarget();
115  bp::list l;
116  for (const double *it = values.getVariablePositions(), *end = it + getVariableCount(); it != end; ++it)
117  l.append(*it);
118  return l;
119  }
120 
121  void rememberJointValuesFromPythonList(const std::string& string, bp::list& values)
122  {
123  rememberJointValues(string, py_bindings_tools::doubleFromList(values));
124  }
125 
126  const char* getPlanningFrameCStr() const
127  {
128  return getPlanningFrame().c_str();
129  }
130 
131  bp::list getActiveJointsList() const
132  {
133  return py_bindings_tools::listFromString(getActiveJoints());
134  }
135 
136  bp::list getJointsList() const
137  {
138  return py_bindings_tools::listFromString(getJoints());
139  }
140 
141  bp::list getCurrentJointValuesList()
142  {
143  return py_bindings_tools::listFromDouble(getCurrentJointValues());
144  }
145 
146  bp::list getRandomJointValuesList()
147  {
148  return py_bindings_tools::listFromDouble(getRandomJointValues());
149  }
150 
151  bp::dict getRememberedJointValuesPython() const
152  {
153  const std::map<std::string, std::vector<double> >& rv = getRememberedJointValues();
154  bp::dict d;
155  for (std::map<std::string, std::vector<double> >::const_iterator it = rv.begin(); it != rv.end(); ++it)
156  d[it->first] = py_bindings_tools::listFromDouble(it->second);
157  return d;
158  }
159 
160  bp::list convertPoseToList(const geometry_msgs::Pose& pose) const
161  {
162  std::vector<double> v(7);
163  v[0] = pose.position.x;
164  v[1] = pose.position.y;
165  v[2] = pose.position.z;
166  v[3] = pose.orientation.x;
167  v[4] = pose.orientation.y;
168  v[5] = pose.orientation.z;
169  v[6] = pose.orientation.w;
171  }
172 
173  bp::list convertTransformToList(const geometry_msgs::Transform& tr) const
174  {
175  std::vector<double> v(7);
176  v[0] = tr.translation.x;
177  v[1] = tr.translation.y;
178  v[2] = tr.translation.z;
179  v[3] = tr.rotation.x;
180  v[4] = tr.rotation.y;
181  v[5] = tr.rotation.z;
182  v[6] = tr.rotation.w;
184  }
185 
186  void convertListToTransform(const bp::list& l, geometry_msgs::Transform& tr) const
187  {
188  std::vector<double> v = py_bindings_tools::doubleFromList(l);
189  tr.translation.x = v[0];
190  tr.translation.y = v[1];
191  tr.translation.z = v[2];
192  tr.rotation.x = v[3];
193  tr.rotation.y = v[4];
194  tr.rotation.z = v[5];
195  tr.rotation.w = v[6];
196  }
197 
198  void convertListToPose(const bp::list& l, geometry_msgs::Pose& p) const
199  {
200  std::vector<double> v = py_bindings_tools::doubleFromList(l);
201  p.position.x = v[0];
202  p.position.y = v[1];
203  p.position.z = v[2];
204  p.orientation.x = v[3];
205  p.orientation.y = v[4];
206  p.orientation.z = v[5];
207  p.orientation.w = v[6];
208  }
209 
210  bp::list getCurrentRPYPython(const std::string& end_effector_link = "")
211  {
212  return py_bindings_tools::listFromDouble(getCurrentRPY(end_effector_link));
213  }
214 
215  bp::list getCurrentPosePython(const std::string& end_effector_link = "")
216  {
217  geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link);
218  return convertPoseToList(pose.pose);
219  }
220 
221  bp::list getRandomPosePython(const std::string& end_effector_link = "")
222  {
223  geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link);
224  return convertPoseToList(pose.pose);
225  }
226 
227  bp::list getKnownConstraintsList() const
228  {
229  return py_bindings_tools::listFromString(getKnownConstraints());
230  }
231 
232  bool placePose(const std::string& object_name, const bp::list& pose)
233  {
234  geometry_msgs::PoseStamped msg;
235  convertListToPose(pose, msg.pose);
236  msg.header.frame_id = getPoseReferenceFrame();
237  msg.header.stamp = ros::Time::now();
238  return place(object_name, msg) == MoveItErrorCode::SUCCESS;
239  }
240 
241  bool placeLocation(const std::string& object_name, const std::string& location_str)
242  {
243  std::vector<moveit_msgs::PlaceLocation> locations(1);
244  py_bindings_tools::deserializeMsg(location_str, locations[0]);
245  return place(object_name, locations) == MoveItErrorCode::SUCCESS;
246  }
247 
248  bool placeAnywhere(const std::string& object_name)
249  {
250  return place(object_name) == MoveItErrorCode::SUCCESS;
251  }
252 
253  void convertListToArrayOfPoses(const bp::list& poses, std::vector<geometry_msgs::Pose>& msg)
254  {
255  int l = bp::len(poses);
256  for (int i = 0; i < l; ++i)
257  {
258  const bp::list& pose = bp::extract<bp::list>(poses[i]);
259  std::vector<double> v = py_bindings_tools::doubleFromList(pose);
260  if (v.size() == 6 || v.size() == 7)
261  {
262  Eigen::Affine3d p;
263  if (v.size() == 6)
264  {
265  Eigen::Quaterniond q;
267  p = Eigen::Affine3d(q);
268  }
269  else
270  p = Eigen::Affine3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5]));
271  p.translation() = Eigen::Vector3d(v[0], v[1], v[2]);
272  geometry_msgs::Pose pm;
273  tf::poseEigenToMsg(p, pm);
274  msg.push_back(pm);
275  }
276  else
277  ROS_WARN("Incorrect number of values for a pose: %u", (unsigned int)v.size());
278  }
279  }
280 
281  bp::dict getCurrentStateBoundedPython()
282  {
283  robot_state::RobotStatePtr current = getCurrentState();
284  current->enforceBounds();
285  moveit_msgs::RobotState rsmv;
286  robot_state::robotStateToRobotStateMsg(*current, rsmv);
287  bp::dict output;
288  for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x)
289  output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x];
290  return output;
291  }
292 
293  void setStartStatePython(const std::string& msg_str)
294  {
295  moveit_msgs::RobotState msg;
296  py_bindings_tools::deserializeMsg(msg_str, msg);
297  setStartState(msg);
298  }
299 
300  bool setPoseTargetsPython(bp::list& poses, const std::string& end_effector_link = "")
301  {
302  std::vector<geometry_msgs::Pose> msg;
303  convertListToArrayOfPoses(poses, msg);
304  return setPoseTargets(msg, end_effector_link);
305  }
306 
307  bool setPoseTargetPython(bp::list& pose, const std::string& end_effector_link = "")
308  {
309  std::vector<double> v = py_bindings_tools::doubleFromList(pose);
310  geometry_msgs::Pose msg;
311  if (v.size() == 6)
312  tf::quaternionTFToMsg(tf::createQuaternionFromRPY(v[3], v[4], v[5]), msg.orientation);
313  else if (v.size() == 7)
314  {
315  msg.orientation.x = v[3];
316  msg.orientation.y = v[4];
317  msg.orientation.z = v[5];
318  msg.orientation.w = v[6];
319  }
320  else
321  {
322  ROS_ERROR("Pose description expected to consist of either 6 or 7 values");
323  return false;
324  }
325  msg.position.x = v[0];
326  msg.position.y = v[1];
327  msg.position.z = v[2];
328  return setPoseTarget(msg, end_effector_link);
329  }
330 
331  const char* getEndEffectorLinkCStr() const
332  {
333  return getEndEffectorLink().c_str();
334  }
335 
336  const char* getPoseReferenceFrameCStr() const
337  {
338  return getPoseReferenceFrame().c_str();
339  }
340 
341  const char* getNameCStr() const
342  {
343  return getName().c_str();
344  }
345 
346  bp::dict getNamedTargetValuesPython(const std::string& name)
347  {
348  bp::dict output;
349  std::map<std::string, double> positions = getNamedTargetValues(name);
350  std::map<std::string, double>::iterator iterator;
351 
352  for (iterator = positions.begin(); iterator != positions.end(); iterator++)
353  output[iterator->first] = iterator->second;
354  return output;
355  }
356 
357  bp::list getNamedTargetsPython()
358  {
359  return py_bindings_tools::listFromString(getNamedTargets());
360  }
361 
362  bool movePython()
363  {
364  return move() == MoveItErrorCode::SUCCESS;
365  }
366 
367  bool asyncMovePython()
368  {
369  return asyncMove() == MoveItErrorCode::SUCCESS;
370  }
371 
372  bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links)
373  {
374  return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links));
375  }
376 
377  bool executePython(const std::string& plan_str)
378  {
379  MoveGroupInterface::Plan plan;
380  py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
381  return execute(plan) == MoveItErrorCode::SUCCESS;
382  }
383 
384  bool asyncExecutePython(const std::string& plan_str)
385  {
386  MoveGroupInterface::Plan plan;
387  py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
388  return asyncExecute(plan) == MoveItErrorCode::SUCCESS;
389  }
390 
391  std::string getPlanPython()
392  {
393  MoveGroupInterface::Plan plan;
395  return py_bindings_tools::serializeMsg(plan.trajectory_);
396  }
397 
398  bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold,
399  bool avoid_collisions)
400  {
401  std::vector<geometry_msgs::Pose> poses;
402  convertListToArrayOfPoses(waypoints, poses);
403  moveit_msgs::RobotTrajectory trajectory;
404  double fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, avoid_collisions);
405  return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction);
406  }
407 
408  int pickGrasp(const std::string& object, const std::string& grasp_str)
409  {
410  moveit_msgs::Grasp grasp;
411  py_bindings_tools::deserializeMsg(grasp_str, grasp);
412  return pick(object, grasp).val;
413  }
414 
415  int pickGrasps(const std::string& object, const bp::list& grasp_list)
416  {
417  int l = bp::len(grasp_list);
418  std::vector<moveit_msgs::Grasp> grasps(l);
419  for (int i = 0; i < l; ++i)
420  py_bindings_tools::deserializeMsg(bp::extract<std::string>(grasp_list[i]), grasps[i]);
421  return pick(object, grasps).val;
422  }
423 
424  void setPathConstraintsFromMsg(const std::string& constraints_str)
425  {
426  moveit_msgs::Constraints constraints_msg;
427  py_bindings_tools::deserializeMsg(constraints_str, constraints_msg);
428  setPathConstraints(constraints_msg);
429  }
430 
431  std::string getPathConstraintsPython()
432  {
433  moveit_msgs::Constraints constraints_msg(getPathConstraints());
434  std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg);
435  return constraints_str;
436  }
437 
438  std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str,
439  double velocity_scaling_factor)
440  {
441  // Convert reference state message to object
442  moveit_msgs::RobotState ref_state_msg;
443  py_bindings_tools::deserializeMsg(ref_state_str, ref_state_msg);
444  moveit::core::RobotState ref_state_obj(getRobotModel());
445  if (moveit::core::robotStateMsgToRobotState(ref_state_msg, ref_state_obj, true))
446  {
447  // Convert trajectory message to object
448  moveit_msgs::RobotTrajectory traj_msg;
449  py_bindings_tools::deserializeMsg(traj_str, traj_msg);
450  robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName());
451  traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg);
452 
453  // Do the actual retiming
455  time_param.computeTimeStamps(traj_obj, velocity_scaling_factor);
456 
457  // Convert the retimed trajectory back into a message
458  traj_obj.getRobotTrajectoryMsg(traj_msg);
459  std::string traj_str = py_bindings_tools::serializeMsg(traj_msg);
460 
461  // Return it.
462  return traj_str;
463  }
464  else
465  {
466  ROS_ERROR("Unable to convert RobotState message to RobotState instance.");
467  return "";
468  }
469  }
470 };
471 
472 class MoveGroupWrapper : public MoveGroupInterfaceWrapper
473 {
474 public:
475  MoveGroupWrapper(const std::string& group_name, const std::string& robot_description)
476  : MoveGroupInterfaceWrapper(group_name, robot_description)
477  {
478  ROS_WARN("The MoveGroup class is deprecated and will be removed in ROS lunar. Please use MoveGroupInterface "
479  "instead.");
480  }
481 };
482 
483 static void wrap_move_group_interface()
484 {
485  bp::class_<MoveGroupInterfaceWrapper, boost::noncopyable> MoveGroupInterfaceClass(
486  "MoveGroupInterface", bp::init<std::string, std::string>());
487 
488  MoveGroupInterfaceClass.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython);
489  MoveGroupInterfaceClass.def("move", &MoveGroupInterfaceWrapper::movePython);
490  MoveGroupInterfaceClass.def("execute", &MoveGroupInterfaceWrapper::executePython);
491  MoveGroupInterfaceClass.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython);
492  moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&) =
493  &MoveGroupInterfaceWrapper::pick;
494  MoveGroupInterfaceClass.def("pick", pick_1);
495  MoveGroupInterfaceClass.def("pick", &MoveGroupInterfaceWrapper::pickGrasp);
496  MoveGroupInterfaceClass.def("pick", &MoveGroupInterfaceWrapper::pickGrasps);
497  MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placePose);
498  MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placeLocation);
499  MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placeAnywhere);
500  MoveGroupInterfaceClass.def("stop", &MoveGroupInterfaceWrapper::stop);
501 
502  MoveGroupInterfaceClass.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr);
503  MoveGroupInterfaceClass.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr);
504 
505  MoveGroupInterfaceClass.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList);
506  MoveGroupInterfaceClass.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList);
507  MoveGroupInterfaceClass.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount);
508  MoveGroupInterfaceClass.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking);
509  MoveGroupInterfaceClass.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning);
510 
511  MoveGroupInterfaceClass.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
512 
513  MoveGroupInterfaceClass.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
514  MoveGroupInterfaceClass.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink);
515  MoveGroupInterfaceClass.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr);
516  MoveGroupInterfaceClass.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr);
517 
518  MoveGroupInterfaceClass.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython);
519 
520  MoveGroupInterfaceClass.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython);
521 
522  MoveGroupInterfaceClass.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget);
523  MoveGroupInterfaceClass.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget);
524  MoveGroupInterfaceClass.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget);
525 
526  MoveGroupInterfaceClass.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython);
527  MoveGroupInterfaceClass.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython);
528 
529  MoveGroupInterfaceClass.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython);
530 
531  MoveGroupInterfaceClass.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget);
532  MoveGroupInterfaceClass.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets);
533 
534  MoveGroupInterfaceClass.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable);
535  MoveGroupInterfaceClass.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict);
536 
537  MoveGroupInterfaceClass.def("set_joint_value_target",
538  &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList);
539  bool (MoveGroupInterfaceWrapper::*setJointValueTarget_4)(const std::string&, double) =
540  &MoveGroupInterfaceWrapper::setJointValueTarget;
541  MoveGroupInterfaceClass.def("set_joint_value_target", setJointValueTarget_4);
542 
543  MoveGroupInterfaceClass.def("set_joint_value_target_from_pose",
544  &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython);
545  MoveGroupInterfaceClass.def("set_joint_value_target_from_pose_stamped",
546  &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython);
547  MoveGroupInterfaceClass.def("set_joint_value_target_from_joint_state_message",
548  &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython);
549 
550  MoveGroupInterfaceClass.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList);
551 
552  MoveGroupInterfaceClass.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget);
553  MoveGroupInterfaceClass.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget);
554 
555  void (MoveGroupInterfaceWrapper::*rememberJointValues_2)(const std::string&) =
556  &MoveGroupInterfaceWrapper::rememberJointValues;
557  MoveGroupInterfaceClass.def("remember_joint_values", rememberJointValues_2);
558 
559  MoveGroupInterfaceClass.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList);
560 
561  MoveGroupInterfaceClass.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor);
562  MoveGroupInterfaceClass.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList);
563  MoveGroupInterfaceClass.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList);
564  MoveGroupInterfaceClass.def("get_remembered_joint_values",
565  &MoveGroupInterfaceWrapper::getRememberedJointValuesPython);
566 
567  MoveGroupInterfaceClass.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues);
568 
569  MoveGroupInterfaceClass.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance);
570  MoveGroupInterfaceClass.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance);
571  MoveGroupInterfaceClass.def("get_goal_orientation_tolerance",
572  &MoveGroupInterfaceWrapper::getGoalOrientationTolerance);
573 
574  MoveGroupInterfaceClass.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance);
575  MoveGroupInterfaceClass.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance);
576  MoveGroupInterfaceClass.def("set_goal_orientation_tolerance",
577  &MoveGroupInterfaceWrapper::setGoalOrientationTolerance);
578  MoveGroupInterfaceClass.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance);
579 
580  MoveGroupInterfaceClass.def("set_start_state_to_current_state",
581  &MoveGroupInterfaceWrapper::setStartStateToCurrentState);
582  MoveGroupInterfaceClass.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython);
583 
584  bool (MoveGroupInterfaceWrapper::*setPathConstraints_1)(const std::string&) =
585  &MoveGroupInterfaceWrapper::setPathConstraints;
586  MoveGroupInterfaceClass.def("set_path_constraints", setPathConstraints_1);
587  MoveGroupInterfaceClass.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg);
588  MoveGroupInterfaceClass.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython);
589  MoveGroupInterfaceClass.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints);
590  MoveGroupInterfaceClass.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList);
591  MoveGroupInterfaceClass.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase);
592  MoveGroupInterfaceClass.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace);
593  MoveGroupInterfaceClass.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime);
594  MoveGroupInterfaceClass.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime);
595  MoveGroupInterfaceClass.def("set_max_velocity_scaling_factor",
596  &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor);
597  MoveGroupInterfaceClass.def("set_max_acceleration_scaling_factor",
598  &MoveGroupWrapper::setMaxAccelerationScalingFactor);
599  MoveGroupInterfaceClass.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId);
600  MoveGroupInterfaceClass.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts);
601  MoveGroupInterfaceClass.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython);
602  MoveGroupInterfaceClass.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython);
603  MoveGroupInterfaceClass.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName);
604  MoveGroupInterfaceClass.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython);
605  MoveGroupInterfaceClass.def("detach_object", &MoveGroupInterfaceWrapper::detachObject);
606  MoveGroupInterfaceClass.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory);
607  MoveGroupInterfaceClass.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython);
608  MoveGroupInterfaceClass.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython);
609  MoveGroupInterfaceClass.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython);
610 
611  bp::class_<MoveGroupWrapper, bp::bases<MoveGroupInterfaceWrapper>, boost::noncopyable> MoveGroupClass(
612  "MoveGroup", bp::init<std::string, std::string>());
613 }
614 }
615 }
616 
617 BOOST_PYTHON_MODULE(_moveit_move_group_interface)
618 {
619  using namespace moveit::planning_interface;
620  wrap_move_group_interface();
621 }
622 
d
Definition: setup.py:4
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
boost::python::list listFromDouble(const std::vector< double > &v)
BOOST_PYTHON_MODULE(_moveit_roscpp_initializer)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void attachObject(void)
Definition: demo.cpp:104
std::string getName(void *handle)
std::vector< std::string > stringFromList(const boost::python::object &values)
Simple interface to MoveIt! components.
#define ROS_WARN(...)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
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)
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
static Time now()
std::string serializeMsg(const T &msg)
Convert a ROS message to a string.
Definition: serialize_msg.h:48
std::vector< double > doubleFromList(const boost::python::object &values)
double x
#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 Mon Jan 15 2018 03:52:20