move_group.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, SRI International
00005  *  Copyright (c) 2012, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan, Sachin Chitta */
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 // for backward compatibility; remove in hydro
00888 namespace move_group_interface = moveit::planning_interface;
00889 
00890 #endif


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:53