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


planning_interface
Author(s): Ioan Sucan
autogenerated on Fri Oct 11 2019 03:57:10