47 #include <boost/python.hpp> 49 #include <boost/shared_ptr.hpp> 60 class MoveGroupInterfaceWrapper :
protected py_bindings_tools::ROScppInitializer,
public MoveGroupInterface
65 MoveGroupInterfaceWrapper(
const std::string& group_name,
const std::string& robot_description,
66 const std::string& ns =
"")
67 : py_bindings_tools::ROScppInitializer()
68 , MoveGroupInterface(Options(group_name, robot_description,
ros::NodeHandle(ns)),
69 boost::shared_ptr<
tf::Transformer>(),
ros::WallDuration(5, 0))
73 bool setJointValueTargetPerJointPythonList(
const std::string& joint, bp::list& values)
78 bool setJointValueTargetPythonIterable(bp::object& values)
83 bool setJointValueTargetPythonDict(bp::dict& values)
85 bp::list k = values.keys();
87 std::map<std::string, double>
v;
88 for (
int i = 0; i < l; ++i)
89 v[bp::extract<std::string>(k[i])] = bp::extract<double>(values[k[i]]);
90 return setJointValueTarget(v);
93 bool setJointValueTargetFromPosePython(
const std::string& pose_str,
const std::string& eef,
bool approx)
95 geometry_msgs::Pose pose_msg;
97 return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
100 bool setJointValueTargetFromPoseStampedPython(
const std::string& pose_str,
const std::string& eef,
bool approx)
102 geometry_msgs::PoseStamped pose_msg;
104 return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef);
107 bool setJointValueTargetFromJointStatePython(
const std::string& js_str)
109 sensor_msgs::JointState js_msg;
111 return setJointValueTarget(js_msg);
114 bool setStateValueTarget(
const std::string& state_str)
116 moveit_msgs::RobotState msg;
123 bp::list getJointValueTargetPythonList()
127 for (
const double *it = values.getVariablePositions(), *end = it + getVariableCount(); it != end; ++it)
132 std::string getJointValueTarget()
134 moveit_msgs::RobotState msg;
140 void rememberJointValuesFromPythonList(
const std::string&
string, bp::list& values)
145 const char* getPlanningFrameCStr()
const 147 return getPlanningFrame().c_str();
150 std::string getInterfaceDescriptionPython()
152 moveit_msgs::PlannerInterfaceDescription msg;
153 getInterfaceDescription(msg);
157 bp::list getActiveJointsList()
const 162 bp::list getJointsList()
const 167 bp::list getCurrentJointValuesList()
172 bp::list getRandomJointValuesList()
177 bp::dict getRememberedJointValuesPython()
const 179 const std::map<std::string, std::vector<double>>& rv = getRememberedJointValues();
181 for (std::map<std::string, std::vector<double>>::const_iterator it = rv.begin(); it != rv.end(); ++it)
186 bp::list convertPoseToList(
const geometry_msgs::Pose& pose)
const 188 std::vector<double> v(7);
189 v[0] = pose.position.x;
190 v[1] = pose.position.y;
191 v[2] = pose.position.z;
192 v[3] = pose.orientation.x;
193 v[4] = pose.orientation.y;
194 v[5] = pose.orientation.z;
195 v[6] = pose.orientation.w;
199 bp::list convertTransformToList(
const geometry_msgs::Transform& tr)
const 201 std::vector<double> v(7);
202 v[0] = tr.translation.x;
203 v[1] = tr.translation.y;
204 v[2] = tr.translation.z;
205 v[3] = tr.rotation.x;
206 v[4] = tr.rotation.y;
207 v[5] = tr.rotation.z;
208 v[6] = tr.rotation.w;
212 void convertListToTransform(
const bp::list& l, geometry_msgs::Transform& tr)
const 215 tr.translation.x = v[0];
216 tr.translation.y = v[1];
217 tr.translation.z = v[2];
218 tr.rotation.x = v[3];
219 tr.rotation.y = v[4];
220 tr.rotation.z = v[5];
221 tr.rotation.w = v[6];
224 void convertListToPose(
const bp::list& l, geometry_msgs::Pose& p)
const 230 p.orientation.x = v[3];
231 p.orientation.y = v[4];
232 p.orientation.z = v[5];
233 p.orientation.w = v[6];
236 bp::list getCurrentRPYPython(
const std::string& end_effector_link =
"")
241 bp::list getCurrentPosePython(
const std::string& end_effector_link =
"")
243 geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link);
244 return convertPoseToList(pose.pose);
247 bp::list getRandomPosePython(
const std::string& end_effector_link =
"")
249 geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link);
250 return convertPoseToList(pose.pose);
253 bp::list getKnownConstraintsList()
const 258 bool placePose(
const std::string& object_name,
const bp::list& pose,
bool plan_only =
false)
260 geometry_msgs::PoseStamped msg;
261 convertListToPose(pose, msg.pose);
262 msg.header.frame_id = getPoseReferenceFrame();
264 return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS;
267 bool placeLocation(
const std::string& object_name,
const std::string& location_str,
bool plan_only =
false)
269 std::vector<moveit_msgs::PlaceLocation> locations(1);
271 return place(object_name, locations, plan_only) == MoveItErrorCode::SUCCESS;
274 bool placeAnywhere(
const std::string& object_name,
bool plan_only =
false)
276 return place(object_name, plan_only) == MoveItErrorCode::SUCCESS;
279 void convertListToArrayOfPoses(
const bp::list& poses, std::vector<geometry_msgs::Pose>& msg)
281 int l = bp::len(poses);
282 for (
int i = 0; i < l; ++i)
284 const bp::list& pose = bp::extract<bp::list>(poses[i]);
286 if (v.size() == 6 || v.size() == 7)
291 Eigen::Quaterniond
q;
293 p = Eigen::Affine3d(q);
296 p = Eigen::Affine3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5]));
297 p.translation() = Eigen::Vector3d(v[0], v[1], v[2]);
298 geometry_msgs::Pose pm;
303 ROS_WARN(
"Incorrect number of values for a pose: %u", (
unsigned int)v.size());
307 bp::dict getCurrentStateBoundedPython()
309 robot_state::RobotStatePtr current = getCurrentState();
310 current->enforceBounds();
311 moveit_msgs::RobotState rsmv;
312 robot_state::robotStateToRobotStateMsg(*current, rsmv);
314 for (
size_t x = 0;
x < rsmv.joint_state.name.size(); ++
x)
315 output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x];
319 void setStartStatePython(
const std::string& msg_str)
321 moveit_msgs::RobotState msg;
326 bool setPoseTargetsPython(bp::list& poses,
const std::string& end_effector_link =
"")
328 std::vector<geometry_msgs::Pose> msg;
329 convertListToArrayOfPoses(poses, msg);
330 return setPoseTargets(msg, end_effector_link);
332 std::string getPoseTargetPython(
const std::string& end_effector_link)
338 bool setPoseTargetPython(bp::list& pose,
const std::string& end_effector_link =
"")
341 geometry_msgs::Pose msg;
344 else if (v.size() == 7)
346 msg.orientation.x = v[3];
347 msg.orientation.y = v[4];
348 msg.orientation.z = v[5];
349 msg.orientation.w = v[6];
353 ROS_ERROR(
"Pose description expected to consist of either 6 or 7 values");
356 msg.position.x = v[0];
357 msg.position.y = v[1];
358 msg.position.z = v[2];
359 return setPoseTarget(msg, end_effector_link);
362 const char* getEndEffectorLinkCStr()
const 364 return getEndEffectorLink().c_str();
367 const char* getPoseReferenceFrameCStr()
const 369 return getPoseReferenceFrame().c_str();
372 const char* getNameCStr()
const 377 bp::dict getNamedTargetValuesPython(
const std::string& name)
380 std::map<std::string, double> positions = getNamedTargetValues(name);
381 std::map<std::string, double>::iterator iterator;
383 for (iterator = positions.begin(); iterator != positions.end(); ++iterator)
384 output[iterator->first] = iterator->second;
388 bp::list getNamedTargetsPython()
395 return move() == MoveItErrorCode::SUCCESS;
398 bool asyncMovePython()
400 return asyncMove() == MoveItErrorCode::SUCCESS;
403 bool attachObjectPython(
const std::string& object_name,
const std::string& link_name,
const bp::list& touch_links)
408 bool executePython(
const std::string& plan_str)
410 MoveGroupInterface::Plan plan;
412 return execute(plan) == MoveItErrorCode::SUCCESS;
415 bool asyncExecutePython(
const std::string& plan_str)
417 MoveGroupInterface::Plan plan;
419 return asyncExecute(plan) == MoveItErrorCode::SUCCESS;
422 std::string getPlanPython()
424 MoveGroupInterface::Plan plan;
429 bp::tuple computeCartesianPathPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
430 bool avoid_collisions)
432 moveit_msgs::Constraints path_constraints_tmp;
433 return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp);
436 bp::tuple computeCartesianPathConstrainedPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
437 bool avoid_collisions,
const std::string& path_constraints_str)
439 moveit_msgs::Constraints path_constraints;
441 return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints);
444 bp::tuple doComputeCartesianPathPython(
const bp::list& waypoints,
double eef_step,
double jump_threshold,
445 bool avoid_collisions,
const moveit_msgs::Constraints& path_constraints)
447 std::vector<geometry_msgs::Pose> poses;
448 convertListToArrayOfPoses(waypoints, poses);
451 computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions);
455 int pickGrasp(
const std::string&
object,
const std::string& grasp_str,
bool plan_only =
false)
457 moveit_msgs::Grasp grasp;
459 return pick(
object, grasp, plan_only).val;
462 int pickGrasps(
const std::string&
object,
const bp::list& grasp_list,
bool plan_only =
false)
464 int l = bp::len(grasp_list);
465 std::vector<moveit_msgs::Grasp> grasps(l);
466 for (
int i = 0; i < l; ++i)
468 return pick(
object, grasps, plan_only).val;
471 void setPathConstraintsFromMsg(
const std::string& constraints_str)
473 moveit_msgs::Constraints constraints_msg;
475 setPathConstraints(constraints_msg);
478 std::string getPathConstraintsPython()
480 moveit_msgs::Constraints constraints_msg(getPathConstraints());
482 return constraints_str;
485 std::string retimeTrajectory(
const std::string& ref_state_str,
const std::string& traj_str,
486 double velocity_scaling_factor)
489 moveit_msgs::RobotState ref_state_msg;
495 moveit_msgs::RobotTrajectory traj_msg;
498 traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg);
505 traj_obj.getRobotTrajectoryMsg(traj_msg);
513 ROS_ERROR(
"Unable to convert RobotState message to RobotState instance.");
518 Eigen::MatrixXd getJacobianMatrixPython(bp::list& joint_values)
521 robot_state::RobotState state(getRobotModel());
522 state.setToDefaultValues();
524 state.setJointGroupPositions(
group, v);
525 return state.getJacobian(
group);
529 class MoveGroupWrapper :
public MoveGroupInterfaceWrapper
532 MoveGroupWrapper(
const std::string& group_name,
const std::string& robot_description,
const std::string& ns =
"")
533 : MoveGroupInterfaceWrapper(group_name, robot_description, ns)
535 ROS_WARN(
"The MoveGroup class is deprecated and will be removed in ROS lunar. Please use MoveGroupInterface " 540 static void wrap_move_group_interface()
542 bp::class_<MoveGroupInterfaceWrapper, boost::noncopyable> MoveGroupInterfaceClass(
543 "MoveGroupInterface", bp::init<std::string, std::string, bp::optional<std::string>>());
546 MoveGroupInterfaceClass.def(
"async_move", &MoveGroupInterfaceWrapper::asyncMovePython);
547 MoveGroupInterfaceClass.def(
"move", &MoveGroupInterfaceWrapper::movePython);
548 MoveGroupInterfaceClass.def(
"execute", &MoveGroupInterfaceWrapper::executePython);
549 MoveGroupInterfaceClass.def(
"async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython);
551 &MoveGroupInterfaceWrapper::pick;
552 MoveGroupInterfaceClass.def(
"pick", pick_1);
553 MoveGroupInterfaceClass.def(
"pick", &MoveGroupInterfaceWrapper::pickGrasp);
554 MoveGroupInterfaceClass.def(
"pick", &MoveGroupInterfaceWrapper::pickGrasps);
555 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placePose);
556 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placeLocation);
557 MoveGroupInterfaceClass.def(
"place", &MoveGroupInterfaceWrapper::placeAnywhere);
558 MoveGroupInterfaceClass.def(
"stop", &MoveGroupInterfaceWrapper::stop);
560 MoveGroupInterfaceClass.def(
"get_name", &MoveGroupInterfaceWrapper::getNameCStr);
561 MoveGroupInterfaceClass.def(
"get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr);
562 MoveGroupInterfaceClass.def(
"get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython);
564 MoveGroupInterfaceClass.def(
"get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList);
565 MoveGroupInterfaceClass.def(
"get_joints", &MoveGroupInterfaceWrapper::getJointsList);
566 MoveGroupInterfaceClass.def(
"get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount);
567 MoveGroupInterfaceClass.def(
"allow_looking", &MoveGroupInterfaceWrapper::allowLooking);
568 MoveGroupInterfaceClass.def(
"allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning);
570 MoveGroupInterfaceClass.def(
"set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
572 MoveGroupInterfaceClass.def(
"set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame);
573 MoveGroupInterfaceClass.def(
"set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink);
574 MoveGroupInterfaceClass.def(
"get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr);
575 MoveGroupInterfaceClass.def(
"get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr);
577 MoveGroupInterfaceClass.def(
"set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython);
579 MoveGroupInterfaceClass.def(
"set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython);
581 MoveGroupInterfaceClass.def(
"set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget);
582 MoveGroupInterfaceClass.def(
"set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget);
583 MoveGroupInterfaceClass.def(
"set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget);
585 MoveGroupInterfaceClass.def(
"get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython);
586 MoveGroupInterfaceClass.def(
"get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython);
588 MoveGroupInterfaceClass.def(
"get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython);
590 MoveGroupInterfaceClass.def(
"clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget);
591 MoveGroupInterfaceClass.def(
"clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets);
593 MoveGroupInterfaceClass.def(
"set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable);
594 MoveGroupInterfaceClass.def(
"set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict);
596 MoveGroupInterfaceClass.def(
"set_joint_value_target",
597 &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList);
598 bool (MoveGroupInterfaceWrapper::*setJointValueTarget_4)(
const std::string&, double) =
599 &MoveGroupInterfaceWrapper::setJointValueTarget;
600 MoveGroupInterfaceClass.def(
"set_joint_value_target", setJointValueTarget_4);
601 MoveGroupInterfaceClass.def(
"set_state_value_target", &MoveGroupInterfaceWrapper::setStateValueTarget);
603 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_pose",
604 &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython);
605 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_pose_stamped",
606 &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython);
607 MoveGroupInterfaceClass.def(
"set_joint_value_target_from_joint_state_message",
608 &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython);
610 MoveGroupInterfaceClass.def(
"get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList);
612 MoveGroupInterfaceClass.def(
"set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget);
613 MoveGroupInterfaceClass.def(
"set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget);
615 void (MoveGroupInterfaceWrapper::*rememberJointValues_2)(
const std::string&) =
616 &MoveGroupInterfaceWrapper::rememberJointValues;
617 MoveGroupInterfaceClass.def(
"remember_joint_values", rememberJointValues_2);
619 MoveGroupInterfaceClass.def(
"remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList);
621 MoveGroupInterfaceClass.def(
"start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor);
622 MoveGroupInterfaceClass.def(
"get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList);
623 MoveGroupInterfaceClass.def(
"get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList);
624 MoveGroupInterfaceClass.def(
"get_remembered_joint_values",
625 &MoveGroupInterfaceWrapper::getRememberedJointValuesPython);
627 MoveGroupInterfaceClass.def(
"forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues);
629 MoveGroupInterfaceClass.def(
"get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance);
630 MoveGroupInterfaceClass.def(
"get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance);
631 MoveGroupInterfaceClass.def(
"get_goal_orientation_tolerance",
632 &MoveGroupInterfaceWrapper::getGoalOrientationTolerance);
634 MoveGroupInterfaceClass.def(
"set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance);
635 MoveGroupInterfaceClass.def(
"set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance);
636 MoveGroupInterfaceClass.def(
"set_goal_orientation_tolerance",
637 &MoveGroupInterfaceWrapper::setGoalOrientationTolerance);
638 MoveGroupInterfaceClass.def(
"set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance);
640 MoveGroupInterfaceClass.def(
"set_start_state_to_current_state",
641 &MoveGroupInterfaceWrapper::setStartStateToCurrentState);
642 MoveGroupInterfaceClass.def(
"set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython);
644 bool (MoveGroupInterfaceWrapper::*setPathConstraints_1)(
const std::string&) =
645 &MoveGroupInterfaceWrapper::setPathConstraints;
646 MoveGroupInterfaceClass.def(
"set_path_constraints", setPathConstraints_1);
647 MoveGroupInterfaceClass.def(
"set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg);
648 MoveGroupInterfaceClass.def(
"get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython);
649 MoveGroupInterfaceClass.def(
"clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints);
650 MoveGroupInterfaceClass.def(
"get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList);
651 MoveGroupInterfaceClass.def(
"set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase);
652 MoveGroupInterfaceClass.def(
"set_workspace", &MoveGroupInterfaceWrapper::setWorkspace);
653 MoveGroupInterfaceClass.def(
"set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime);
654 MoveGroupInterfaceClass.def(
"get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime);
655 MoveGroupInterfaceClass.def(
"set_max_velocity_scaling_factor",
656 &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor);
657 MoveGroupInterfaceClass.def(
"set_max_acceleration_scaling_factor",
658 &MoveGroupWrapper::setMaxAccelerationScalingFactor);
659 MoveGroupInterfaceClass.def(
"set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId);
660 MoveGroupInterfaceClass.def(
"set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts);
661 MoveGroupInterfaceClass.def(
"compute_plan", &MoveGroupInterfaceWrapper::getPlanPython);
662 MoveGroupInterfaceClass.def(
"compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython);
663 MoveGroupInterfaceClass.def(
"compute_cartesian_path",
664 &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython);
665 MoveGroupInterfaceClass.def(
"set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName);
666 MoveGroupInterfaceClass.def(
"attach_object", &MoveGroupInterfaceWrapper::attachObjectPython);
667 MoveGroupInterfaceClass.def(
"detach_object", &MoveGroupInterfaceWrapper::detachObject);
668 MoveGroupInterfaceClass.def(
"retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory);
669 MoveGroupInterfaceClass.def(
"get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython);
670 MoveGroupInterfaceClass.def(
"get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython);
671 MoveGroupInterfaceClass.def(
"get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython);
672 MoveGroupInterfaceClass.def(
"get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython);
674 bp::class_<MoveGroupWrapper, bp::bases<MoveGroupInterfaceWrapper>, boost::noncopyable> MoveGroupClass(
675 "MoveGroup", bp::init<std::string, std::string>());
683 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)
void EIGENPY_DLLAPI enableEigenPy()
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