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/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 // for backward compatibility; remove in hydro
00763 namespace move_group_interface=moveit::planning_interface;
00764 
00765 #endif


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:44:13