47 #include <boost/python.hpp> 48 #include <boost/shared_ptr.hpp> 53 namespace bp = boost::python;
59 class MoveGroupInterfaceWrapper :
protected py_bindings_tools::ROScppInitializer,
public MoveGroupInterface
64 MoveGroupInterfaceWrapper(
const std::string& group_name,
const std::string& robot_description,
65 const std::string& ns =
"")
66 : py_bindings_tools::ROScppInitializer()
67 , MoveGroupInterface(Options(group_name, robot_description,
ros::NodeHandle(ns)),
68 boost::shared_ptr<
tf::Transformer>(),
ros::WallDuration(5, 0))
72 bool setJointValueTargetPerJointPythonList(
const std::string& joint, bp::list& values)
77 bool setJointValueTargetPythonIterable(bp::object& values)
82 bool setJointValueTargetPythonDict(bp::dict& values)
84 bp::list k = values.keys();
86 std::map<std::string, double> v;
87 for (
int i = 0; i < l; ++i)
88 v[bp::extract<std::string>(k[i])] = bp::extract<double>(values[k[i]]);
89 return setJointValueTarget(v);
92 bool setJointValueTargetFromPosePython(
const std::string& pose_str,
const std::string& eef,
bool approx)
94 geometry_msgs::Pose pose_msg;
96 return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
99 bool setJointValueTargetFromPoseStampedPython(
const std::string& pose_str,
const std::string& eef,
bool approx)
101 geometry_msgs::PoseStamped pose_msg;
103 return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
106 bool setJointValueTargetFromJointStatePython(
const std::string& js_str)
108 sensor_msgs::JointState js_msg;
110 return setJointValueTarget(js_msg);
113 bool setStateValueTarget(
const std::string& state_str)
115 moveit_msgs::RobotState msg;
122 bp::list getJointValueTargetPythonList()
126 for (
const double *it = values.getVariablePositions(), *end = it + getVariableCount(); it != end; ++it)
131 std::string getJointValueTarget()
133 moveit_msgs::RobotState msg;
139 void rememberJointValuesFromPythonList(
const std::string&
string, bp::list& values)
144 const char* getPlanningFrameCStr()
const 146 return getPlanningFrame().c_str();
149 std::string getInterfaceDescriptionPython()
151 moveit_msgs::PlannerInterfaceDescription msg;
152 getInterfaceDescription(msg);
156 bp::list getActiveJointsList()
const 161 bp::list getJointsList()
const 166 bp::list getCurrentJointValuesList()
171 bp::list getRandomJointValuesList()
176 bp::dict getRememberedJointValuesPython()
const 178 const std::map<std::string, std::vector<double> >& rv = getRememberedJointValues();
180 for (std::map<std::string, std::vector<double> >::const_iterator it = rv.begin(); it != rv.end(); ++it)
185 bp::list convertPoseToList(
const geometry_msgs::Pose& pose)
const 187 std::vector<double> v(7);
188 v[0] = pose.position.x;
189 v[1] = pose.position.y;
190 v[2] = pose.position.z;
191 v[3] = pose.orientation.x;
192 v[4] = pose.orientation.y;
193 v[5] = pose.orientation.z;
194 v[6] = pose.orientation.w;
198 bp::list convertTransformToList(
const geometry_msgs::Transform& tr)
const 200 std::vector<double> v(7);
201 v[0] = tr.translation.x;
202 v[1] = tr.translation.y;
203 v[2] = tr.translation.z;
204 v[3] = tr.rotation.x;
205 v[4] = tr.rotation.y;
206 v[5] = tr.rotation.z;
207 v[6] = tr.rotation.w;
211 void convertListToTransform(
const bp::list& l, geometry_msgs::Transform& tr)
const 214 tr.translation.x = v[0];
215 tr.translation.y = v[1];
216 tr.translation.z = v[2];
217 tr.rotation.x = v[3];
218 tr.rotation.y = v[4];
219 tr.rotation.z = v[5];
220 tr.rotation.w = v[6];
223 void convertListToPose(
const bp::list& l, geometry_msgs::Pose& p)
const 229 p.orientation.x = v[3];
230 p.orientation.y = v[4];
231 p.orientation.z = v[5];
232 p.orientation.w = v[6];
235 bp::list getCurrentRPYPython(
const std::string& end_effector_link =
"")
240 bp::list getCurrentPosePython(
const std::string& end_effector_link =
"")
242 geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link);
243 return convertPoseToList(pose.pose);
246 bp::list getRandomPosePython(
const std::string& end_effector_link =
"")
248 geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link);
249 return convertPoseToList(pose.pose);
252 bp::list getKnownConstraintsList()
const 257 bool placePose(
const std::string& object_name,
const bp::list& pose)
259 geometry_msgs::PoseStamped msg;
260 convertListToPose(pose, msg.pose);
261 msg.header.frame_id = getPoseReferenceFrame();
263 return place(object_name, msg) == MoveItErrorCode::SUCCESS;
266 bool placeLocation(
const std::string& object_name,
const std::string& location_str)
268 std::vector<moveit_msgs::PlaceLocation> locations(1);
270 return place(object_name, locations) == MoveItErrorCode::SUCCESS;
273 bool placeAnywhere(
const std::string& object_name)
275 return place(object_name) == MoveItErrorCode::SUCCESS;
278 void convertListToArrayOfPoses(
const bp::list& poses, std::vector<geometry_msgs::Pose>& msg)
280 int l = bp::len(poses);
281 for (
int i = 0; i < l; ++i)
283 const bp::list& pose = bp::extract<bp::list>(poses[i]);
285 if (v.size() == 6 || v.size() == 7)
290 Eigen::Quaterniond q;
292 p = Eigen::Affine3d(q);
295 p = Eigen::Affine3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5]));
296 p.translation() = Eigen::Vector3d(v[0], v[1], v[2]);
297 geometry_msgs::Pose pm;
302 ROS_WARN(
"Incorrect number of values for a pose: %u", (
unsigned int)v.size());
306 bp::dict getCurrentStateBoundedPython()
308 robot_state::RobotStatePtr current = getCurrentState();
309 current->enforceBounds();
310 moveit_msgs::RobotState rsmv;
311 robot_state::robotStateToRobotStateMsg(*current, rsmv);
313 for (
size_t x = 0;
x < rsmv.joint_state.name.size(); ++
x)
314 output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x];
318 void setStartStatePython(
const std::string& msg_str)
320 moveit_msgs::RobotState msg;
325 bool setPoseTargetsPython(bp::list& poses,
const std::string& end_effector_link =
"")
327 std::vector<geometry_msgs::Pose> msg;
328 convertListToArrayOfPoses(poses, msg);
329 return setPoseTargets(msg, end_effector_link);
331 std::string getPoseTargetPython(
const std::string& end_effector_link)
337 bool setPoseTargetPython(bp::list& pose,
const std::string& end_effector_link =
"")
340 geometry_msgs::Pose msg;
343 else if (v.size() == 7)
345 msg.orientation.x = v[3];
346 msg.orientation.y = v[4];
347 msg.orientation.z = v[5];
348 msg.orientation.w = v[6];
352 ROS_ERROR(
"Pose description expected to consist of either 6 or 7 values");
355 msg.position.x = v[0];
356 msg.position.y = v[1];
357 msg.position.z = v[2];
358 return setPoseTarget(msg, end_effector_link);
361 const char* getEndEffectorLinkCStr()
const 363 return getEndEffectorLink().c_str();
366 const char* getPoseReferenceFrameCStr()
const 368 return getPoseReferenceFrame().c_str();
371 const char* getNameCStr()
const 376 bp::dict getNamedTargetValuesPython(
const std::string& name)
379 std::map<std::string, double> positions = getNamedTargetValues(name);
380 std::map<std::string, double>::iterator iterator;
382 for (iterator = positions.begin(); iterator != positions.end(); ++iterator)
383 output[iterator->first] = iterator->second;
387 bp::list getNamedTargetsPython()
394 return move() == MoveItErrorCode::SUCCESS;
397 bool asyncMovePython()
399 return asyncMove() == MoveItErrorCode::SUCCESS;
402 bool attachObjectPython(
const std::string& object_name,
const std::string& link_name,
const bp::list& touch_links)
407 bool executePython(
const std::string& plan_str)
409 MoveGroupInterface::Plan plan;
411 return execute(plan) == MoveItErrorCode::SUCCESS;
414 bool asyncExecutePython(
const std::string& plan_str)
416 MoveGroupInterface::Plan plan;
418 return asyncExecute(plan) == MoveItErrorCode::SUCCESS;
421 std::string getPlanPython()
423 MoveGroupInterface::Plan plan;
428 bp::tuple computeCartesianPathPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
429 bool avoid_collisions)
431 moveit_msgs::Constraints path_constraints_tmp;
432 return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp);
435 bp::tuple computeCartesianPathConstrainedPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
436 bool avoid_collisions,
const std::string& path_constraints_str)
438 moveit_msgs::Constraints path_constraints;
440 return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints);
443 bp::tuple doComputeCartesianPathPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
444 bool avoid_collisions,
const moveit_msgs::Constraints& path_constraints)
446 std::vector<geometry_msgs::Pose> poses;
447 convertListToArrayOfPoses(waypoints, poses);
450 computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions);
454 int pickGrasp(
const std::string&
object,
const std::string& grasp_str)
456 moveit_msgs::Grasp grasp;
458 return pick(
object, grasp).val;
461 int pickGrasps(
const std::string&
object,
const bp::list& grasp_list)
463 int l = bp::len(grasp_list);
464 std::vector<moveit_msgs::Grasp> grasps(l);
465 for (
int i = 0; i < l; ++i)
467 return pick(
object, grasps).val;
470 void setPathConstraintsFromMsg(
const std::string& constraints_str)
472 moveit_msgs::Constraints constraints_msg;
474 setPathConstraints(constraints_msg);
477 std::string getPathConstraintsPython()
479 moveit_msgs::Constraints constraints_msg(getPathConstraints());
481 return constraints_str;
484 std::string retimeTrajectory(
const std::string& ref_state_str,
const std::string& traj_str,
485 double velocity_scaling_factor)
488 moveit_msgs::RobotState ref_state_msg;
494 moveit_msgs::RobotTrajectory traj_msg;
497 traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg);
504 traj_obj.getRobotTrajectoryMsg(traj_msg);
512 ROS_ERROR(
"Unable to convert RobotState message to RobotState instance.");
518 class MoveGroupWrapper :
public MoveGroupInterfaceWrapper
521 MoveGroupWrapper(
const std::string& group_name,
const std::string& robot_description,
const std::string& ns =
"")
522 : MoveGroupInterfaceWrapper(group_name, robot_description, ns)
524 ROS_WARN(
"The MoveGroup class is deprecated and will be removed in ROS lunar. Please use MoveGroupInterface " 529 static void wrap_move_group_interface()
531 bp::class_<MoveGroupInterfaceWrapper, boost::noncopyable> MoveGroupInterfaceClass(
532 "MoveGroupInterface", bp::init<std::string, std::string, std::string>());
534 MoveGroupInterfaceClass.def(
"async_move", &MoveGroupInterfaceWrapper::asyncMovePython);
535 MoveGroupInterfaceClass.def(
"move", &MoveGroupInterfaceWrapper::movePython);
536 MoveGroupInterfaceClass.def(
"execute", &MoveGroupInterfaceWrapper::executePython);
537 MoveGroupInterfaceClass.def(
"async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython);
539 &MoveGroupInterfaceWrapper::pick;
540 MoveGroupInterfaceClass.def(
"pick", pick_1);
541 MoveGroupInterfaceClass.def(
"pick", &MoveGroupInterfaceWrapper::pickGrasp);
542 MoveGroupInterfaceClass.def(
"pick", &MoveGroupInterfaceWrapper::pickGrasps);
543 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placePose);
544 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placeLocation);
545 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placeAnywhere);
546 MoveGroupInterfaceClass.def(
"stop", &MoveGroupInterfaceWrapper::stop);
548 MoveGroupInterfaceClass.def(
"get_name", &MoveGroupInterfaceWrapper::getNameCStr);
549 MoveGroupInterfaceClass.def(
"get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr);
550 MoveGroupInterfaceClass.def(
"get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython);
552 MoveGroupInterfaceClass.def(
"get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList);
553 MoveGroupInterfaceClass.def(
"get_joints", &MoveGroupInterfaceWrapper::getJointsList);
554 MoveGroupInterfaceClass.def(
"get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount);
555 MoveGroupInterfaceClass.def(
"allow_looking", &MoveGroupInterfaceWrapper::allowLooking);
556 MoveGroupInterfaceClass.def(
"allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning);
558 MoveGroupInterfaceClass.def(
"set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
560 MoveGroupInterfaceClass.def(
"set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
561 MoveGroupInterfaceClass.def(
"set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink);
562 MoveGroupInterfaceClass.def(
"get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr);
563 MoveGroupInterfaceClass.def(
"get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr);
565 MoveGroupInterfaceClass.def(
"set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython);
567 MoveGroupInterfaceClass.def(
"set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython);
569 MoveGroupInterfaceClass.def(
"set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget);
570 MoveGroupInterfaceClass.def(
"set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget);
571 MoveGroupInterfaceClass.def(
"set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget);
573 MoveGroupInterfaceClass.def(
"get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython);
574 MoveGroupInterfaceClass.def(
"get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython);
576 MoveGroupInterfaceClass.def(
"get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython);
578 MoveGroupInterfaceClass.def(
"clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget);
579 MoveGroupInterfaceClass.def(
"clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets);
581 MoveGroupInterfaceClass.def(
"set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable);
582 MoveGroupInterfaceClass.def(
"set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict);
584 MoveGroupInterfaceClass.def(
"set_joint_value_target",
585 &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList);
586 bool (MoveGroupInterfaceWrapper::*setJointValueTarget_4)(
const std::string&, double) =
587 &MoveGroupInterfaceWrapper::setJointValueTarget;
588 MoveGroupInterfaceClass.def(
"set_joint_value_target", setJointValueTarget_4);
589 MoveGroupInterfaceClass.def(
"set_state_value_target", &MoveGroupInterfaceWrapper::setStateValueTarget);
591 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_pose",
592 &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython);
593 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_pose_stamped",
594 &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython);
595 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_joint_state_message",
596 &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython);
598 MoveGroupInterfaceClass.def(
"get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList);
600 MoveGroupInterfaceClass.def(
"set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget);
601 MoveGroupInterfaceClass.def(
"set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget);
603 void (MoveGroupInterfaceWrapper::*rememberJointValues_2)(
const std::string&) =
604 &MoveGroupInterfaceWrapper::rememberJointValues;
605 MoveGroupInterfaceClass.def(
"remember_joint_values", rememberJointValues_2);
607 MoveGroupInterfaceClass.def(
"remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList);
609 MoveGroupInterfaceClass.def(
"start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor);
610 MoveGroupInterfaceClass.def(
"get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList);
611 MoveGroupInterfaceClass.def(
"get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList);
612 MoveGroupInterfaceClass.def(
"get_remembered_joint_values",
613 &MoveGroupInterfaceWrapper::getRememberedJointValuesPython);
615 MoveGroupInterfaceClass.def(
"forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues);
617 MoveGroupInterfaceClass.def(
"get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance);
618 MoveGroupInterfaceClass.def(
"get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance);
619 MoveGroupInterfaceClass.def(
"get_goal_orientation_tolerance",
620 &MoveGroupInterfaceWrapper::getGoalOrientationTolerance);
622 MoveGroupInterfaceClass.def(
"set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance);
623 MoveGroupInterfaceClass.def(
"set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance);
624 MoveGroupInterfaceClass.def(
"set_goal_orientation_tolerance",
625 &MoveGroupInterfaceWrapper::setGoalOrientationTolerance);
626 MoveGroupInterfaceClass.def(
"set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance);
628 MoveGroupInterfaceClass.def(
"set_start_state_to_current_state",
629 &MoveGroupInterfaceWrapper::setStartStateToCurrentState);
630 MoveGroupInterfaceClass.def(
"set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython);
632 bool (MoveGroupInterfaceWrapper::*setPathConstraints_1)(
const std::string&) =
633 &MoveGroupInterfaceWrapper::setPathConstraints;
634 MoveGroupInterfaceClass.def(
"set_path_constraints", setPathConstraints_1);
635 MoveGroupInterfaceClass.def(
"set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg);
636 MoveGroupInterfaceClass.def(
"get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython);
637 MoveGroupInterfaceClass.def(
"clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints);
638 MoveGroupInterfaceClass.def(
"get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList);
639 MoveGroupInterfaceClass.def(
"set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase);
640 MoveGroupInterfaceClass.def(
"set_workspace", &MoveGroupInterfaceWrapper::setWorkspace);
641 MoveGroupInterfaceClass.def(
"set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime);
642 MoveGroupInterfaceClass.def(
"get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime);
643 MoveGroupInterfaceClass.def(
"set_max_velocity_scaling_factor",
644 &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor);
645 MoveGroupInterfaceClass.def(
"set_max_acceleration_scaling_factor",
646 &MoveGroupWrapper::setMaxAccelerationScalingFactor);
647 MoveGroupInterfaceClass.def(
"set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId);
648 MoveGroupInterfaceClass.def(
"set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts);
649 MoveGroupInterfaceClass.def(
"compute_plan", &MoveGroupInterfaceWrapper::getPlanPython);
650 MoveGroupInterfaceClass.def(
"compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython);
651 MoveGroupInterfaceClass.def(
"compute_cartesian_path",
652 &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython);
653 MoveGroupInterfaceClass.def(
"set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName);
654 MoveGroupInterfaceClass.def(
"attach_object", &MoveGroupInterfaceWrapper::attachObjectPython);
655 MoveGroupInterfaceClass.def(
"detach_object", &MoveGroupInterfaceWrapper::detachObject);
656 MoveGroupInterfaceClass.def(
"retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory);
657 MoveGroupInterfaceClass.def(
"get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython);
658 MoveGroupInterfaceClass.def(
"get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython);
659 MoveGroupInterfaceClass.def(
"get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython);
661 bp::class_<MoveGroupWrapper, bp::bases<MoveGroupInterfaceWrapper>, boost::noncopyable> MoveGroupClass(
662 "MoveGroup", bp::init<std::string, std::string>());
670 wrap_move_group_interface();
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
BOOST_PYTHON_MODULE(_moveit_roscpp_initializer)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
std::string getName(void *handle)
Simple interface to MoveIt! components.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const