00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_
00039 #define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_
00040
00041 #include <moveit/robot_state/robot_state.h>
00042 #include <moveit_msgs/RobotTrajectory.h>
00043 #include <moveit_msgs/RobotState.h>
00044 #include <moveit_msgs/PlannerInterfaceDescription.h>
00045 #include <moveit_msgs/Constraints.h>
00046 #include <moveit_msgs/Grasp.h>
00047 #include <moveit_msgs/PlaceLocation.h>
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <boost/shared_ptr.hpp>
00050 #include <tf/tf.h>
00051
00052 namespace moveit
00053 {
00054
00056 namespace planning_interface
00057 {
00058
00059 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
00060 {
00061 public:
00062 MoveItErrorCode() { val = 0; };
00063 MoveItErrorCode(int code) { val = code; };
00064 MoveItErrorCode(const moveit_msgs::MoveItErrorCodes &code) { val = code.val; };
00065 operator bool() const { return val == moveit_msgs::MoveItErrorCodes::SUCCESS; }
00066 };
00067
00069 class MoveGroup
00070 {
00071 public:
00072
00074 static const std::string ROBOT_DESCRIPTION;
00075
00077 struct Options
00078 {
00079 Options(const std::string &group_name,
00080 const std::string &desc = ROBOT_DESCRIPTION,
00081 const ros::NodeHandle &node_handle = ros::NodeHandle()) :
00082 group_name_(group_name),
00083 robot_description_(desc),
00084 node_handle_(node_handle)
00085 {
00086 }
00087
00089 std::string group_name_;
00090
00092 std::string robot_description_;
00093
00095 robot_model::RobotModelConstPtr robot_model_;
00096
00097 ros::NodeHandle node_handle_;
00098 };
00099
00101 struct Plan
00102 {
00104 moveit_msgs::RobotState start_state_;
00105
00107 moveit_msgs::RobotTrajectory trajectory_;
00108
00110 double planning_time_;
00111 };
00112
00116 MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00117 const ros::Duration &wait_for_server = ros::Duration(0, 0));
00118
00122 MoveGroup(const std::string &group, const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00123 const ros::Duration &wait_for_server = ros::Duration(0, 0));
00124
00125 ~MoveGroup();
00126
00128 const std::string& getName() const;
00129
00131 const std::string& getPlanningFrame() const;
00132
00134 const std::vector<std::string>& getActiveJoints() const;
00135
00137 const std::vector<std::string>& getJoints() const;
00138
00140 unsigned int getVariableCount() const;
00141
00143 bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc);
00144
00146 void setPlannerId(const std::string &planner_id);
00147
00149 void setPlanningTime(double seconds);
00150
00152 void setNumPlanningAttempts(unsigned int num_planning_attempts);
00153
00155 double getPlanningTime() const;
00156
00158 double getGoalJointTolerance() const;
00159
00161 double getGoalPositionTolerance() const;
00162
00164 double getGoalOrientationTolerance() const;
00165
00172 void setGoalTolerance(double tolerance);
00173
00175 void setGoalJointTolerance(double tolerance);
00176
00178 void setGoalPositionTolerance(double tolerance);
00179
00181 void setGoalOrientationTolerance(double tolerance);
00182
00186 void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
00187
00189 void setStartState(const moveit_msgs::RobotState &start_state);
00190
00192 void setStartState(const robot_state::RobotState &start_state);
00193
00195 void setStartStateToCurrentState();
00196
00198 void setSupportSurfaceName(const std::string &name);
00199
00228 bool setJointValueTarget(const std::vector<double> &group_variable_values);
00229
00245 bool setJointValueTarget(const std::map<std::string, double> &variable_values);
00246
00256 bool setJointValueTarget(const robot_state::RobotState &robot_state);
00257
00269 bool setJointValueTarget(const std::string &joint_name, const std::vector<double> &values);
00270
00282 bool setJointValueTarget(const std::string &joint_name, double value);
00283
00294 bool setJointValueTarget(const sensor_msgs::JointState &state);
00295
00307 bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link = "");
00308
00320 bool setJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link = "");
00321
00333 bool setJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link = "");
00334
00345 bool setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link = "");
00346
00357 bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link = "");
00358
00369 bool setApproximateJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link = "");
00370
00375 void setRandomTarget();
00376
00379 bool setNamedTarget(const std::string &name);
00380
00382 const robot_state::RobotState& getJointValueTarget() const;
00383
00406 bool setPositionTarget(double x, double y, double z, const std::string &end_effector_link = "");
00407
00415 bool setRPYTarget(double roll, double pitch, double yaw, const std::string &end_effector_link = "");
00416
00424 bool setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link = "");
00425
00433 bool setPoseTarget(const Eigen::Affine3d &end_effector_pose, const std::string &end_effector_link = "");
00434
00442 bool setPoseTarget(const geometry_msgs::Pose &target, const std::string &end_effector_link = "");
00443
00451 bool setPoseTarget(const geometry_msgs::PoseStamped &target, const std::string &end_effector_link = "");
00452
00471 bool setPoseTargets(const EigenSTL::vector_Affine3d &end_effector_pose, const std::string &end_effector_link = "");
00472
00491 bool setPoseTargets(const std::vector<geometry_msgs::Pose> &target, const std::string &end_effector_link = "");
00492
00511 bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &target, const std::string &end_effector_link = "");
00512
00514 void setPoseReferenceFrame(const std::string &pose_reference_frame);
00515
00519 bool setEndEffectorLink(const std::string &end_effector_link);
00520
00523 bool setEndEffector(const std::string &eef_name);
00524
00526 void clearPoseTarget(const std::string &end_effector_link = "");
00527
00529 void clearPoseTargets();
00530
00535 const geometry_msgs::PoseStamped& getPoseTarget(const std::string &end_effector_link = "") const;
00536
00540 const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string &end_effector_link = "") const;
00541
00547 const std::string& getEndEffectorLink() const;
00548
00553 const std::string& getEndEffector() const;
00554
00556 const std::string& getPoseReferenceFrame() const;
00557
00567 MoveItErrorCode asyncMove();
00568
00571 MoveItErrorCode move();
00572
00575 MoveItErrorCode plan(Plan &plan);
00576
00578 MoveItErrorCode asyncExecute(const Plan &plan);
00579
00581 MoveItErrorCode execute(const Plan &plan);
00582
00590 double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double eef_step, double jump_threshold,
00591 moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes *error_code = NULL);
00592
00594 void stop();
00595
00597 void allowLooking(bool flag);
00598
00600 void allowReplanning(bool flag);
00601
00610 MoveItErrorCode pick(const std::string &object);
00611
00613 MoveItErrorCode pick(const std::string &object, const moveit_msgs::Grasp &grasp);
00614
00616 MoveItErrorCode pick(const std::string &object, const std::vector<moveit_msgs::Grasp> &grasps);
00617
00619 MoveItErrorCode place(const std::string &object);
00620
00622 MoveItErrorCode place(const std::string &object, const std::vector<moveit_msgs::PlaceLocation> &locations);
00623
00625 MoveItErrorCode place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses);
00626
00628 MoveItErrorCode place(const std::string &object, const geometry_msgs::PoseStamped &pose);
00629
00636 bool attachObject(const std::string &object, const std::string &link = "");
00637
00646 bool attachObject(const std::string &object, const std::string &link, const std::vector<std::string> &touch_links);
00647
00653 bool detachObject(const std::string &name = "");
00654
00667 bool startStateMonitor(double wait = 1.0);
00668
00670 std::vector<double> getCurrentJointValues();
00671
00673 robot_state::RobotStatePtr getCurrentState();
00674
00677 geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link = "");
00678
00681 std::vector<double> getCurrentRPY(const std::string &end_effector_link = "");
00682
00684 std::vector<double> getRandomJointValues();
00685
00688 geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link = "");
00689
00701 void rememberJointValues(const std::string &name);
00702
00707 void rememberJointValues(const std::string &name, const std::vector<double> &values);
00708
00710 const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
00711 {
00712 return remembered_joint_values_;
00713 }
00714
00716 void forgetJointValues(const std::string &name);
00717
00726 void setConstraintsDatabase(const std::string &host, unsigned int port);
00727
00729 std::vector<std::string> getKnownConstraints() const;
00730
00734 moveit_msgs::Constraints getPathConstraints() const;
00735
00739 bool setPathConstraints(const std::string &constraint);
00740
00744 void setPathConstraints(const moveit_msgs::Constraints &constraint);
00745
00748 void clearPathConstraints();
00749
00752 private:
00753
00754 std::map<std::string, std::vector<double> > remembered_joint_values_;
00755 class MoveGroupImpl;
00756 MoveGroupImpl *impl_;
00757
00758 };
00759
00760 }
00761 }
00762
00763 namespace move_group_interface=moveit::planning_interface;
00764
00765 #endif