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/macros/class_forward.h>
00042 #include <moveit/robot_state/robot_state.h>
00043 #include <moveit_msgs/RobotTrajectory.h>
00044 #include <moveit_msgs/RobotState.h>
00045 #include <moveit_msgs/PlannerInterfaceDescription.h>
00046 #include <moveit_msgs/Constraints.h>
00047 #include <moveit_msgs/Grasp.h>
00048 #include <moveit_msgs/PlaceLocation.h>
00049 #include <geometry_msgs/PoseStamped.h>
00050 #include <boost/shared_ptr.hpp>
00051 #include <tf/tf.h>
00052
00053 namespace moveit
00054 {
00056 namespace planning_interface
00057 {
00058 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
00059 {
00060 public:
00061 MoveItErrorCode()
00062 {
00063 val = 0;
00064 };
00065 MoveItErrorCode(int code)
00066 {
00067 val = code;
00068 };
00069 MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
00070 {
00071 val = code.val;
00072 };
00073 operator bool() const
00074 {
00075 return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00076 }
00077 };
00078
00079 MOVEIT_CLASS_FORWARD(MoveGroup);
00080
00083 class MoveGroup
00084 {
00085 public:
00087 static const std::string ROBOT_DESCRIPTION;
00088
00090 struct Options
00091 {
00092 Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION,
00093 const ros::NodeHandle& node_handle = ros::NodeHandle())
00094 : group_name_(group_name), robot_description_(desc), node_handle_(node_handle)
00095 {
00096 }
00097
00099 std::string group_name_;
00100
00102 std::string robot_description_;
00103
00105 robot_model::RobotModelConstPtr robot_model_;
00106
00107 ros::NodeHandle node_handle_;
00108 };
00109
00110 MOVEIT_STRUCT_FORWARD(Plan);
00111
00113 struct Plan
00114 {
00116 moveit_msgs::RobotState start_state_;
00117
00119 moveit_msgs::RobotTrajectory trajectory_;
00120
00122 double planning_time_;
00123 };
00124
00135 MoveGroup(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00136 const ros::WallDuration& wait_for_servers = ros::WallDuration());
00137 MoveGroup(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf, const ros::Duration& wait_for_servers);
00138
00149 MoveGroup(const std::string& group,
00150 const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00151 const ros::WallDuration& wait_for_servers = ros::WallDuration());
00152 MoveGroup(const std::string& group, const boost::shared_ptr<tf::Transformer>& tf,
00153 const ros::Duration& wait_for_servers);
00154
00155 ~MoveGroup();
00156
00158 const std::string& getName() const;
00159
00162 const std::vector<std::string> getNamedTargets();
00163
00165 robot_model::RobotModelConstPtr getRobotModel() const;
00166
00168 const ros::NodeHandle& getNodeHandle() const;
00169
00171 const std::string& getPlanningFrame() const;
00172
00174 const std::vector<std::string>& getJointNames();
00175
00177 const std::vector<std::string>& getLinkNames();
00178
00180 std::map<std::string, double> getNamedTargetValues(const std::string& name);
00181
00183 const std::vector<std::string>& getActiveJoints() const;
00184
00186 const std::vector<std::string>& getJoints() const;
00187
00190 unsigned int getVariableCount() const;
00191
00193 bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc);
00194
00196 std::string getDefaultPlannerId(const std::string& group = "") const;
00197
00199 void setPlannerId(const std::string& planner_id);
00200
00202 void setPlanningTime(double seconds);
00203
00206 void setNumPlanningAttempts(unsigned int num_planning_attempts);
00207
00213 void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
00214
00220 void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
00221
00223 double getPlanningTime() const;
00224
00227 double getGoalJointTolerance() const;
00228
00231 double getGoalPositionTolerance() const;
00232
00235 double getGoalOrientationTolerance() const;
00236
00243 void setGoalTolerance(double tolerance);
00244
00247 void setGoalJointTolerance(double tolerance);
00248
00250 void setGoalPositionTolerance(double tolerance);
00251
00253 void setGoalOrientationTolerance(double tolerance);
00254
00259 void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
00260
00263 void setStartState(const moveit_msgs::RobotState& start_state);
00264
00267 void setStartState(const robot_state::RobotState& start_state);
00268
00270 void setStartStateToCurrentState();
00271
00274 void setSupportSurfaceName(const std::string& name);
00275
00306 bool setJointValueTarget(const std::vector<double>& group_variable_values);
00307
00323 bool setJointValueTarget(const std::map<std::string, double>& variable_values);
00324
00334 bool setJointValueTarget(const robot_state::RobotState& robot_state);
00335
00347 bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
00348
00360 bool setJointValueTarget(const std::string& joint_name, double value);
00361
00372 bool setJointValueTarget(const sensor_msgs::JointState& state);
00373
00385 bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
00386
00398 bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
00399
00411 bool setJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
00412
00423 bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
00424
00435 bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
00436 const std::string& end_effector_link = "");
00437
00448 bool setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
00449
00454 void setRandomTarget();
00455
00458 bool setNamedTarget(const std::string& name);
00459
00461 const robot_state::RobotState& getJointValueTarget() const;
00462
00484 bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
00485
00493 bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
00494
00503 bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
00504
00512 bool setPoseTarget(const Eigen::Affine3d& end_effector_pose, const std::string& end_effector_link = "");
00513
00521 bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
00522
00530 bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
00531
00550 bool setPoseTargets(const EigenSTL::vector_Affine3d& end_effector_pose, const std::string& end_effector_link = "");
00551
00570 bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
00571
00590 bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
00591
00593 void setPoseReferenceFrame(const std::string& pose_reference_frame);
00594
00598 bool setEndEffectorLink(const std::string& end_effector_link);
00599
00602 bool setEndEffector(const std::string& eef_name);
00603
00605 void clearPoseTarget(const std::string& end_effector_link = "");
00606
00608 void clearPoseTargets();
00609
00616 const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
00617
00623 const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
00624
00630 const std::string& getEndEffectorLink() const;
00631
00637 const std::string& getEndEffector() const;
00638
00641 const std::string& getPoseReferenceFrame() const;
00642
00653 MoveItErrorCode asyncMove();
00654
00659 MoveItErrorCode move();
00660
00664 MoveItErrorCode plan(Plan& plan);
00665
00667 MoveItErrorCode asyncExecute(const Plan& plan);
00668
00670 MoveItErrorCode execute(const Plan& plan);
00671
00681 double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
00682 moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
00683 moveit_msgs::MoveItErrorCodes* error_code = NULL);
00684
00697 double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
00698 moveit_msgs::RobotTrajectory& trajectory,
00699 const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
00700 moveit_msgs::MoveItErrorCodes* error_code = NULL);
00701
00703 void stop();
00704
00707 void allowLooking(bool flag);
00708
00710 void allowReplanning(bool flag);
00711
00723 MoveItErrorCode pick(const std::string& object);
00724
00726 MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp);
00727
00731 MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps);
00732
00736 MoveItErrorCode planGraspsAndPick(const std::string& object);
00737
00741 MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object);
00742
00744 MoveItErrorCode place(const std::string& object);
00745
00747 MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations);
00748
00750 MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses);
00751
00753 MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose);
00754
00761 bool attachObject(const std::string& object, const std::string& link = "");
00762
00771 bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
00772
00778 bool detachObject(const std::string& name = "");
00779
00792 bool startStateMonitor(double wait = 1.0);
00793
00795 std::vector<double> getCurrentJointValues();
00796
00798 robot_state::RobotStatePtr getCurrentState();
00799
00803 geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "");
00804
00808 std::vector<double> getCurrentRPY(const std::string& end_effector_link = "");
00809
00811 std::vector<double> getRandomJointValues();
00812
00816 geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "");
00817
00829 void rememberJointValues(const std::string& name);
00830
00835 void rememberJointValues(const std::string& name, const std::vector<double>& values);
00836
00838 const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
00839 {
00840 return remembered_joint_values_;
00841 }
00842
00844 void forgetJointValues(const std::string& name);
00845
00854 void setConstraintsDatabase(const std::string& host, unsigned int port);
00855
00857 std::vector<std::string> getKnownConstraints() const;
00858
00862 moveit_msgs::Constraints getPathConstraints() const;
00863
00867 bool setPathConstraints(const std::string& constraint);
00868
00872 void setPathConstraints(const moveit_msgs::Constraints& constraint);
00873
00876 void clearPathConstraints();
00877
00880 private:
00881 std::map<std::string, std::vector<double> > remembered_joint_values_;
00882 class MoveGroupImpl;
00883 MoveGroupImpl* impl_;
00884 };
00885 }
00886 }
00887
00888 namespace move_group_interface = moveit::planning_interface;
00889
00890 #endif