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