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