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/macros/deprecation.h>
00043 #include <moveit/robot_state/robot_state.h>
00044 #include <moveit_msgs/RobotTrajectory.h>
00045 #include <moveit_msgs/RobotState.h>
00046 #include <moveit_msgs/PlannerInterfaceDescription.h>
00047 #include <moveit_msgs/Constraints.h>
00048 #include <moveit_msgs/Grasp.h>
00049 #include <moveit_msgs/PlaceLocation.h>
00050 #include <geometry_msgs/PoseStamped.h>
00051 #include <boost/shared_ptr.hpp>
00052 #include <tf/tf.h>
00053 
00054 namespace moveit
00055 {
00057 namespace planning_interface
00058 {
00059 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
00060 {
00061 public:
00062   MoveItErrorCode()
00063   {
00064     val = 0;
00065   };
00066   MoveItErrorCode(int code)
00067   {
00068     val = code;
00069   };
00070   MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
00071   {
00072     val = code.val;
00073   };
00074   operator bool() const
00075   {
00076     return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00077   }
00078 };
00079 
00080 MOVEIT_CLASS_FORWARD(MoveGroup);
00081 
00084 class MoveGroup
00085 {
00086 public:
00088   static const std::string ROBOT_DESCRIPTION;
00089 
00091   struct Options
00092   {
00093     Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION,
00094             const ros::NodeHandle& node_handle = ros::NodeHandle())
00095       : group_name_(group_name), robot_description_(desc), node_handle_(node_handle)
00096     {
00097     }
00098 
00100     std::string group_name_;
00101 
00103     std::string robot_description_;
00104 
00106     robot_model::RobotModelConstPtr robot_model_;
00107 
00108     ros::NodeHandle node_handle_;
00109   };
00110 
00111   MOVEIT_STRUCT_FORWARD(Plan);
00112 
00114   struct Plan
00115   {
00117     moveit_msgs::RobotState start_state_;
00118 
00120     moveit_msgs::RobotTrajectory trajectory_;
00121 
00123     double planning_time_;
00124   };
00125 
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   MOVEIT_DEPRECATED MoveGroup(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
00138                               const ros::Duration& wait_for_servers);
00139 
00146   MoveGroup(const std::string& group,
00147             const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00148             const ros::WallDuration& wait_for_servers = ros::WallDuration());
00149   MOVEIT_DEPRECATED MoveGroup(const std::string& group, const boost::shared_ptr<tf::Transformer>& tf,
00150                               const ros::Duration& wait_for_servers);
00151 
00152   ~MoveGroup();
00153 
00155   const std::string& getName() const;
00156 
00159   const std::vector<std::string> getNamedTargets();
00160 
00162   robot_model::RobotModelConstPtr getRobotModel() const;
00163 
00165   const ros::NodeHandle& getNodeHandle() const;
00166 
00168   const std::string& getPlanningFrame() const;
00169 
00171   const std::vector<std::string>& getJointNames();
00172 
00174   const std::vector<std::string>& getLinkNames();
00175 
00177   std::map<std::string, double> getNamedTargetValues(const std::string& name);
00178 
00180   const std::vector<std::string>& getActiveJoints() const;
00181 
00183   const std::vector<std::string>& getJoints() const;
00184 
00187   unsigned int getVariableCount() const;
00188 
00190   bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc);
00191 
00193   std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "");
00194 
00196   void setPlannerParams(const std::string& planner_id, const std::string& group,
00197                         const std::map<std::string, std::string>& params, bool bReplace = false);
00198 
00200   std::string getDefaultPlannerId(const std::string& group = "") const;
00201 
00203   void setPlannerId(const std::string& planner_id);
00204 
00206   void setPlanningTime(double seconds);
00207 
00210   void setNumPlanningAttempts(unsigned int num_planning_attempts);
00211 
00217   void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
00218 
00224   void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
00225 
00227   double getPlanningTime() const;
00228 
00231   double getGoalJointTolerance() const;
00232 
00235   double getGoalPositionTolerance() const;
00236 
00239   double getGoalOrientationTolerance() const;
00240 
00247   void setGoalTolerance(double tolerance);
00248 
00251   void setGoalJointTolerance(double tolerance);
00252 
00254   void setGoalPositionTolerance(double tolerance);
00255 
00257   void setGoalOrientationTolerance(double tolerance);
00258 
00263   void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
00264 
00267   void setStartState(const moveit_msgs::RobotState& start_state);
00268 
00271   void setStartState(const robot_state::RobotState& start_state);
00272 
00274   void setStartStateToCurrentState();
00275 
00278   void setSupportSurfaceName(const std::string& name);
00279 
00310   bool setJointValueTarget(const std::vector<double>& group_variable_values);
00311 
00327   bool setJointValueTarget(const std::map<std::string, double>& variable_values);
00328 
00338   bool setJointValueTarget(const robot_state::RobotState& robot_state);
00339 
00351   bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
00352 
00364   bool setJointValueTarget(const std::string& joint_name, double value);
00365 
00376   bool setJointValueTarget(const sensor_msgs::JointState& state);
00377 
00389   bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
00390 
00402   bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
00403 
00415   bool setJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
00416 
00427   bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
00428 
00439   bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
00440                                       const std::string& end_effector_link = "");
00441 
00452   bool setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
00453 
00458   void setRandomTarget();
00459 
00462   bool setNamedTarget(const std::string& name);
00463 
00465   const robot_state::RobotState& getJointValueTarget() const;
00466 
00488   bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
00489 
00497   bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
00498 
00507   bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
00508 
00516   bool setPoseTarget(const Eigen::Affine3d& end_effector_pose, const std::string& end_effector_link = "");
00517 
00525   bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
00526 
00534   bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
00535 
00554   bool setPoseTargets(const EigenSTL::vector_Affine3d& end_effector_pose, const std::string& end_effector_link = "");
00555 
00574   bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
00575 
00594   bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
00595 
00597   void setPoseReferenceFrame(const std::string& pose_reference_frame);
00598 
00602   bool setEndEffectorLink(const std::string& end_effector_link);
00603 
00606   bool setEndEffector(const std::string& eef_name);
00607 
00609   void clearPoseTarget(const std::string& end_effector_link = "");
00610 
00612   void clearPoseTargets();
00613 
00620   const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
00621 
00627   const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
00628 
00634   const std::string& getEndEffectorLink() const;
00635 
00641   const std::string& getEndEffector() const;
00642 
00645   const std::string& getPoseReferenceFrame() const;
00646 
00657   MoveItErrorCode asyncMove();
00658 
00663   MoveItErrorCode move();
00664 
00668   MoveItErrorCode plan(Plan& plan);
00669 
00671   MoveItErrorCode asyncExecute(const Plan& plan);
00672 
00674   MoveItErrorCode execute(const Plan& plan);
00675 
00685   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
00686                               moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
00687                               moveit_msgs::MoveItErrorCodes* error_code = NULL);
00688 
00701   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
00702                               moveit_msgs::RobotTrajectory& trajectory,
00703                               const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
00704                               moveit_msgs::MoveItErrorCodes* error_code = NULL);
00705 
00707   void stop();
00708 
00711   void allowLooking(bool flag);
00712 
00714   void allowReplanning(bool flag);
00715 
00727   MoveItErrorCode pick(const std::string& object);
00728 
00730   MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp);
00731 
00735   MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps);
00736 
00740   MoveItErrorCode planGraspsAndPick(const std::string& object);
00741 
00745   MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object);
00746 
00748   MoveItErrorCode place(const std::string& object);
00749 
00751   MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations);
00752 
00754   MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses);
00755 
00757   MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose);
00758 
00765   bool attachObject(const std::string& object, const std::string& link = "");
00766 
00775   bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
00776 
00782   bool detachObject(const std::string& name = "");
00783 
00796   bool startStateMonitor(double wait = 1.0);
00797 
00799   std::vector<double> getCurrentJointValues();
00800 
00802   robot_state::RobotStatePtr getCurrentState();
00803 
00807   geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "");
00808 
00812   std::vector<double> getCurrentRPY(const std::string& end_effector_link = "");
00813 
00815   std::vector<double> getRandomJointValues();
00816 
00820   geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "");
00821 
00833   void rememberJointValues(const std::string& name);
00834 
00839   void rememberJointValues(const std::string& name, const std::vector<double>& values);
00840 
00842   const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
00843   {
00844     return remembered_joint_values_;
00845   }
00846 
00848   void forgetJointValues(const std::string& name);
00849 
00858   void setConstraintsDatabase(const std::string& host, unsigned int port);
00859 
00861   std::vector<std::string> getKnownConstraints() const;
00862 
00866   moveit_msgs::Constraints getPathConstraints() const;
00867 
00871   bool setPathConstraints(const std::string& constraint);
00872 
00876   void setPathConstraints(const moveit_msgs::Constraints& constraint);
00877 
00880   void clearPathConstraints();
00881 
00884 private:
00885   std::map<std::string, std::vector<double> > remembered_joint_values_;
00886   class MoveGroupImpl;
00887   MoveGroupImpl* impl_;
00888 };
00889 }
00890 }
00891 // for backward compatibility; remove in hydro
00892 namespace move_group_interface = moveit::planning_interface;
00893 
00894 #endif


planning_interface
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:22:06