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