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