move_group_interface.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, SRI International
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Sachin Chitta */
37 
38 #ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_
39 #define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_
40 
44 #include <moveit_msgs/RobotTrajectory.h>
45 #include <moveit_msgs/RobotState.h>
46 #include <moveit_msgs/PlannerInterfaceDescription.h>
47 #include <moveit_msgs/Constraints.h>
48 #include <moveit_msgs/Grasp.h>
49 #include <moveit_msgs/PlaceLocation.h>
50 #include <moveit_msgs/MotionPlanRequest.h>
51 #include <geometry_msgs/PoseStamped.h>
52 #include <boost/shared_ptr.hpp>
53 #include <tf/tf.h>
54 
55 namespace moveit
56 {
58 namespace planning_interface
59 {
60 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
61 {
62 public:
64  {
65  val = 0;
66  }
67  MoveItErrorCode(int code)
68  {
69  val = code;
70  }
71  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
72  {
73  val = code.val;
74  }
75  explicit operator bool() const
76  {
77  return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
78  }
79  bool operator==(const int c) const
80  {
81  return val == c;
82  }
83  bool operator!=(const int c) const
84  {
85  return val != c;
86  }
87 };
88 
90 
97 {
98 public:
100  static const std::string ROBOT_DESCRIPTION;
101 
103  struct Options
104  {
105  Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION,
106  const ros::NodeHandle& node_handle = ros::NodeHandle())
107  : group_name_(group_name), robot_description_(desc), node_handle_(node_handle)
108  {
109  }
110 
112  std::string group_name_;
113 
115  std::string robot_description_;
116 
118  robot_model::RobotModelConstPtr robot_model_;
119 
121  };
122 
124 
126  struct Plan
127  {
129  moveit_msgs::RobotState start_state_;
130 
132  moveit_msgs::RobotTrajectory trajectory_;
133 
136  };
137 
147  MoveGroupInterface(const Options& opt,
149  const ros::WallDuration& wait_for_servers = ros::WallDuration());
151  const ros::Duration& wait_for_servers);
152 
159  MoveGroupInterface(const std::string& group,
161  const ros::WallDuration& wait_for_servers = ros::WallDuration());
163  const ros::Duration& wait_for_servers);
164 
166 
172  MoveGroupInterface(const MoveGroupInterface&) = delete;
173  MoveGroupInterface& operator=(const MoveGroupInterface&) = delete;
174 
176  MoveGroupInterface& operator=(MoveGroupInterface&& other);
177 
179  const std::string& getName() const;
180 
183  const std::vector<std::string> getNamedTargets();
184 
186  robot_model::RobotModelConstPtr getRobotModel() const;
187 
189  const ros::NodeHandle& getNodeHandle() const;
190 
192  const std::string& getPlanningFrame() const;
193 
195  const std::vector<std::string>& getJointNames();
196 
198  const std::vector<std::string>& getLinkNames();
199 
201  std::map<std::string, double> getNamedTargetValues(const std::string& name);
202 
204  const std::vector<std::string>& getActiveJoints() const;
205 
207  const std::vector<std::string>& getJoints() const;
208 
211  unsigned int getVariableCount() const;
212 
214  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc);
215 
217  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "");
218 
220  void setPlannerParams(const std::string& planner_id, const std::string& group,
221  const std::map<std::string, std::string>& params, bool bReplace = false);
222 
224  std::string getDefaultPlannerId(const std::string& group = "") const;
225 
227  void setPlannerId(const std::string& planner_id);
228 
230  void setPlanningTime(double seconds);
231 
234  void setNumPlanningAttempts(unsigned int num_planning_attempts);
235 
241  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
242 
248  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
249 
251  double getPlanningTime() const;
252 
255  double getGoalJointTolerance() const;
256 
259  double getGoalPositionTolerance() const;
260 
263  double getGoalOrientationTolerance() const;
264 
271  void setGoalTolerance(double tolerance);
272 
275  void setGoalJointTolerance(double tolerance);
276 
278  void setGoalPositionTolerance(double tolerance);
279 
281  void setGoalOrientationTolerance(double tolerance);
282 
287  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
288 
291  void setStartState(const moveit_msgs::RobotState& start_state);
292 
295  void setStartState(const robot_state::RobotState& start_state);
296 
298  void setStartStateToCurrentState();
299 
302  void setSupportSurfaceName(const std::string& name);
303 
334  bool setJointValueTarget(const std::vector<double>& group_variable_values);
335 
351  bool setJointValueTarget(const std::map<std::string, double>& variable_values);
352 
362  bool setJointValueTarget(const robot_state::RobotState& robot_state);
363 
375  bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
376 
388  bool setJointValueTarget(const std::string& joint_name, double value);
389 
400  bool setJointValueTarget(const sensor_msgs::JointState& state);
401 
413  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
414 
426  bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
427 
439  bool setJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
440 
451  bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
452 
463  bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
464  const std::string& end_effector_link = "");
465 
476  bool setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
477 
482  void setRandomTarget();
483 
486  bool setNamedTarget(const std::string& name);
487 
489  const robot_state::RobotState& getJointValueTarget() const;
490 
512  bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
513 
521  bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
522 
531  bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
532 
540  bool setPoseTarget(const Eigen::Affine3d& end_effector_pose, const std::string& end_effector_link = "");
541 
549  bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
550 
558  bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
559 
578  bool setPoseTargets(const EigenSTL::vector_Affine3d& end_effector_pose, const std::string& end_effector_link = "");
579 
598  bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
599 
618  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
619 
621  void setPoseReferenceFrame(const std::string& pose_reference_frame);
622 
626  bool setEndEffectorLink(const std::string& end_effector_link);
627 
630  bool setEndEffector(const std::string& eef_name);
631 
633  void clearPoseTarget(const std::string& end_effector_link = "");
634 
636  void clearPoseTargets();
637 
644  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
645 
651  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
652 
658  const std::string& getEndEffectorLink() const;
659 
665  const std::string& getEndEffector() const;
666 
669  const std::string& getPoseReferenceFrame() const;
670 
681  MoveItErrorCode asyncMove();
682 
687  MoveItErrorCode move();
688 
692  MoveItErrorCode plan(Plan& plan);
693 
695  MoveItErrorCode asyncExecute(const Plan& plan);
696 
698  MoveItErrorCode execute(const Plan& plan);
699 
709  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
710  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
711  moveit_msgs::MoveItErrorCodes* error_code = NULL);
712 
725  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
726  moveit_msgs::RobotTrajectory& trajectory,
727  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
728  moveit_msgs::MoveItErrorCodes* error_code = NULL);
729 
731  void stop();
732 
735  void allowLooking(bool flag);
736 
738  void allowReplanning(bool flag);
739 
742  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request);
743 
754  MoveItErrorCode pick(const std::string& object);
755 
757  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp);
758 
762  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps);
763 
767  MoveItErrorCode planGraspsAndPick(const std::string& object = "");
768 
772  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object);
773 
775  MoveItErrorCode place(const std::string& object);
776 
778  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations);
779 
781  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses);
782 
784  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose);
785 
792  bool attachObject(const std::string& object, const std::string& link = "");
793 
802  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
803 
809  bool detachObject(const std::string& name = "");
810 
823  bool startStateMonitor(double wait = 1.0);
824 
826  std::vector<double> getCurrentJointValues();
827 
829  robot_state::RobotStatePtr getCurrentState();
830 
834  geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "");
835 
839  std::vector<double> getCurrentRPY(const std::string& end_effector_link = "");
840 
842  std::vector<double> getRandomJointValues();
843 
847  geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "");
848 
860  void rememberJointValues(const std::string& name);
861 
866  void rememberJointValues(const std::string& name, const std::vector<double>& values);
867 
869  const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
870  {
871  return remembered_joint_values_;
872  }
873 
875  void forgetJointValues(const std::string& name);
876 
885  void setConstraintsDatabase(const std::string& host, unsigned int port);
886 
888  std::vector<std::string> getKnownConstraints() const;
889 
893  moveit_msgs::Constraints getPathConstraints() const;
894 
898  bool setPathConstraints(const std::string& constraint);
899 
903  void setPathConstraints(const moveit_msgs::Constraints& constraint);
904 
907  void clearPathConstraints();
908 
911 private:
912  std::map<std::string, std::vector<double> > remembered_joint_values_;
915 };
916 }
917 }
918 
919 #endif
MoveItErrorCode(const moveit_msgs::MoveItErrorCodes &code)
const std::map< std::string, std::vector< double > > & getRememberedJointValues() const
Get the currently remembered map of names to joint values.
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_) ...
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot&#39;s URDF. Set to &#39;robot_description&#39;.
desc
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
moveit::planning_interface::MoveGroup MOVEIT_DEPRECATED
void attachObject(void)
Definition: demo.cpp:104
std::map< std::string, std::vector< double > > remembered_joint_values_
std::string getName(void *handle)
Specification of options to use when constructing the MoveGroupInterface class.
std::string robot_description_
The robot description parameter name (if different from default)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
#define MOVEIT_STRUCT_FORWARD(C)
Options(const std::string &group_name, const std::string &desc=ROBOT_DESCRIPTION, const ros::NodeHandle &node_handle=ros::NodeHandle())
Client class to conveniently use the ROS interfaces provided by the move_group node.
robot_model::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
std::string group_name_
The group to construct the class instance for.
double planning_time_
The amount of time it took to generate the plan.
The representation of a motion plan (as ROS messasges)
MOVEIT_CLASS_FORWARD(MoveGroupInterface)


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Jan 21 2018 03:56:10