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 #pragma once
39 
42 #include <moveit_msgs/RobotTrajectory.h>
43 #include <moveit_msgs/RobotState.h>
44 #include <moveit_msgs/PlannerInterfaceDescription.h>
45 #include <moveit_msgs/Constraints.h>
46 #include <moveit_msgs/Grasp.h>
47 #include <moveit_msgs/PlaceLocation.h>
48 #include <moveit_msgs/PickupGoal.h>
49 #include <moveit_msgs/PlaceGoal.h>
50 #include <moveit_msgs/MotionPlanRequest.h>
51 #include <moveit_msgs/MoveGroupAction.h>
52 #include <geometry_msgs/PoseStamped.h>
54 #include <memory>
55 #include <utility>
56 #include <tf2_ros/buffer.h>
57 
58 namespace moveit
59 {
61 namespace planning_interface
62 {
63 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
64 {
65 public:
67  {
68  val = 0;
69  }
70  MoveItErrorCode(int code)
71  {
72  val = code;
73  }
74  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
75  {
76  val = code.val;
77  }
78  explicit operator bool() const
79  {
80  return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
81  }
82  bool operator==(const int c) const
83  {
84  return val == c;
85  }
86  bool operator!=(const int c) const
87  {
88  return val != c;
89  }
90 };
91 
92 MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc
93 
99 class MoveGroupInterface
100 {
101 public:
103  static const std::string ROBOT_DESCRIPTION;
104 
106  struct Options
107  {
108  Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION,
109  const ros::NodeHandle& node_handle = ros::NodeHandle())
110  : group_name_(std::move(group_name)), robot_description_(std::move(desc)), node_handle_(node_handle)
111  {
112  }
113 
115  std::string group_name_;
116 
118  std::string robot_description_;
119 
121  moveit::core::RobotModelConstPtr robot_model_;
122 
124  };
125 
126  MOVEIT_STRUCT_FORWARD(Plan);
127 
129  struct Plan
130  {
132  moveit_msgs::RobotState start_state_;
133 
135  moveit_msgs::RobotTrajectory trajectory_;
136 
138  double planning_time_;
139  };
140 
151  MoveGroupInterface(const Options& opt,
152  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
153  const ros::WallDuration& wait_for_servers = ros::WallDuration());
154  [[deprecated]] MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
155  const ros::Duration& wait_for_servers);
156 
164  MoveGroupInterface(const std::string& group,
165  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
166  const ros::WallDuration& wait_for_servers = ros::WallDuration());
167  [[deprecated]] MoveGroupInterface(const std::string& group, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
168  const ros::Duration& wait_for_servers);
169 
171 
179 
180  MoveGroupInterface(MoveGroupInterface&& other) noexcept;
183  const std::string& getName() const;
184 
187  const std::vector<std::string>& getNamedTargets() const;
188 
190  moveit::core::RobotModelConstPtr getRobotModel() const;
191 
193  const ros::NodeHandle& getNodeHandle() const;
194 
196  const std::string& getPlanningFrame() const;
197 
199  const std::vector<std::string>& getJointModelGroupNames() const;
200 
202  const std::vector<std::string>& getJointNames() const;
203 
205  const std::vector<std::string>& getLinkNames() const;
206 
208  std::map<std::string, double> getNamedTargetValues(const std::string& name) const;
209 
211  const std::vector<std::string>& getActiveJoints() const;
212 
214  const std::vector<std::string>& getJoints() const;
215 
218  unsigned int getVariableCount() const;
219 
221  bool getInterfaceDescriptions(std::vector<moveit_msgs::PlannerInterfaceDescription>& desc) const;
222 
224  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const;
225 
227  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id,
228  const std::string& group = "") const;
229 
231  void setPlannerParams(const std::string& planner_id, const std::string& group,
232  const std::map<std::string, std::string>& params, bool bReplace = false);
233 
234  std::string getDefaultPlanningPipelineId() const;
235 
237  void setPlanningPipelineId(const std::string& pipeline_id);
238 
240  const std::string& getPlanningPipelineId() const;
241 
243  std::string getDefaultPlannerId(const std::string& group = "") const;
244 
246  void setPlannerId(const std::string& planner_id);
247 
249  const std::string& getPlannerId() const;
250 
252  void setPlanningTime(double seconds);
253 
256  void setNumPlanningAttempts(unsigned int num_planning_attempts);
257 
263  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
264 
270  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
271 
273  double getPlanningTime() const;
274 
277  double getGoalJointTolerance() const;
278 
281  double getGoalPositionTolerance() const;
282 
285  double getGoalOrientationTolerance() const;
286 
293  void setGoalTolerance(double tolerance);
294 
297  void setGoalJointTolerance(double tolerance);
298 
300  void setGoalPositionTolerance(double tolerance);
301 
303  void setGoalOrientationTolerance(double tolerance);
304 
309  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
310 
313  void setStartState(const moveit_msgs::RobotState& start_state);
314 
317  void setStartState(const moveit::core::RobotState& start_state);
318 
321 
324  void setSupportSurfaceName(const std::string& name);
325 
355  bool setJointValueTarget(const std::vector<double>& group_variable_values);
356 
372  bool setJointValueTarget(const std::map<std::string, double>& variable_values);
373 
389  bool setJointValueTarget(const std::vector<std::string>& variable_names, const std::vector<double>& variable_values);
390 
400  bool setJointValueTarget(const moveit::core::RobotState& robot_state);
401 
413  bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
414 
426  bool setJointValueTarget(const std::string& joint_name, double value);
427 
438  bool setJointValueTarget(const sensor_msgs::JointState& state);
439 
451  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
452 
464  bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
465 
477  bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
478 
489  bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
490 
501  bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
502  const std::string& end_effector_link = "");
503 
514  bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
515 
520  void setRandomTarget();
521 
524  bool setNamedTarget(const std::string& name);
525 
527  void getJointValueTarget(std::vector<double>& group_variable_values) const;
528 
530  [[deprecated]] const moveit::core::RobotState& getJointValueTarget() const;
531 
553  bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
554 
562  bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
563 
572  bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
573 
581  bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
582 
590  bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
591 
599  bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
600 
619  bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
620 
639  bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
640 
659  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
660 
662  void setPoseReferenceFrame(const std::string& pose_reference_frame);
663 
667  bool setEndEffectorLink(const std::string& end_effector_link);
668 
671  bool setEndEffector(const std::string& eef_name);
672 
674  void clearPoseTarget(const std::string& end_effector_link = "");
675 
677  void clearPoseTargets();
678 
685  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
686 
692  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
693 
699  const std::string& getEndEffectorLink() const;
700 
706  const std::string& getEndEffector() const;
707 
710  const std::string& getPoseReferenceFrame() const;
711 
723 
728 
734 
739 
742 
744  MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory);
745 
748 
750  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory);
751 
761  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
762  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
763  moveit_msgs::MoveItErrorCodes* error_code = nullptr);
764 
777  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
778  moveit_msgs::RobotTrajectory& trajectory,
779  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
780  moveit_msgs::MoveItErrorCodes* error_code = nullptr);
781 
783  void stop();
784 
786  void allowReplanning(bool flag);
787 
789  void setReplanAttempts(int32_t attempts);
790 
792  void setReplanDelay(double delay);
793 
796  void allowLooking(bool flag);
797 
799  void setLookAroundAttempts(int32_t attempts);
800 
803  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request);
804 
806  moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp> grasps,
807  bool plan_only) const;
808 
810  moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
811  std::vector<moveit_msgs::PlaceLocation> locations, bool plan_only) const;
812 
814  std::vector<moveit_msgs::PlaceLocation>
815  posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses) const;
816 
827  MoveItErrorCode pick(const std::string& object, bool plan_only = false)
828  {
829  return pick(constructPickupGoal(object, std::vector<moveit_msgs::Grasp>(), plan_only));
830  }
831 
833  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
834  {
835  return pick(constructPickupGoal(object, { grasp }, plan_only));
836  }
837 
839  MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
840  {
841  return pick(constructPickupGoal(object, std::move(grasps), plan_only));
842  }
843 
848  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);
849 
853  MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
854 
858  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
859 
861  MoveItErrorCode place(const std::string& object, bool plan_only = false)
862  {
863  return place(constructPlaceGoal(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only));
864  }
865 
867  MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
868  bool plan_only = false)
869  {
870  return place(constructPlaceGoal(object, std::move(locations), plan_only));
871  }
872 
874  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
875  bool plan_only = false)
876  {
877  return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
878  }
879 
881  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false)
882  {
883  return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
884  }
885 
890  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);
891 
898  bool attachObject(const std::string& object, const std::string& link = "");
899 
908  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
909 
915  bool detachObject(const std::string& name = "");
916 
929  bool startStateMonitor(double wait = 1.0);
930 
932  std::vector<double> getCurrentJointValues() const;
933 
935  moveit::core::RobotStatePtr getCurrentState(double wait = 1) const;
936 
940  geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const;
941 
945  std::vector<double> getCurrentRPY(const std::string& end_effector_link = "") const;
946 
948  std::vector<double> getRandomJointValues() const;
949 
953  geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "") const;
954 
966  void rememberJointValues(const std::string& name);
967 
972  void rememberJointValues(const std::string& name, const std::vector<double>& values);
973 
975  const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
976  {
978  }
979 
981  void forgetJointValues(const std::string& name);
982 
991  void setConstraintsDatabase(const std::string& host, unsigned int port);
992 
994  std::vector<std::string> getKnownConstraints() const;
995 
999  moveit_msgs::Constraints getPathConstraints() const;
1000 
1004  bool setPathConstraints(const std::string& constraint);
1005 
1009  void setPathConstraints(const moveit_msgs::Constraints& constraint);
1010 
1013  void clearPathConstraints();
1014 
1015  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const;
1016  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint);
1018 
1021 protected:
1024 
1025 private:
1026  std::map<std::string, std::vector<double> > remembered_joint_values_;
1027  class MoveGroupInterfaceImpl;
1028  MoveGroupInterfaceImpl* impl_;
1029 };
1030 } // namespace planning_interface
1031 } // namespace moveit
moveit::planning_interface::MoveGroupInterface::place
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
Definition: move_group_interface.h:927
moveit::planning_interface::MoveGroupInterface::getEndEffector
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
Definition: move_group_interface.cpp:1878
moveit::planning_interface::MoveGroupInterface::setStartState
void setStartState(const moveit_msgs::RobotState &start_state)
If a different start state should be considered instead of the current state of the robot,...
Definition: move_group_interface.cpp:1662
moveit::planning_interface::MoveGroupInterface::getPlannerParams
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="") const
Get the planner parameters for given group and planner_id.
Definition: move_group_interface.cpp:1493
moveit::planning_interface::MoveGroupInterface::plan
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
Definition: move_group_interface.cpp:1585
moveit::planning_interface::MoveGroupInterface::setPoseTarget
bool setPoseTarget(const Eigen::Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set the goal pose of the end-effector end_effector_link.
Definition: move_group_interface.cpp:1910
moveit::planning_interface::MoveGroupInterface::setJointValueTarget
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
Definition: move_group_interface.cpp:1743
moveit::planning_interface::MoveGroupInterface::getKnownConstraints
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved.
Definition: move_group_interface.cpp:2310
moveit::planning_interface::MoveGroupInterface::getDefaultPlanningPipelineId
std::string getDefaultPlanningPipelineId() const
Definition: move_group_interface.cpp:1505
moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
Definition: move_group_interface.cpp:2335
moveit::planning_interface::MoveGroupInterface::getJoints
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
Definition: move_group_interface.cpp:2232
moveit::planning_interface::MoveGroupInterface::getMoveGroupClient
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
Definition: move_group_interface.cpp:1555
moveit::planning_interface::MoveGroupInterface::startStateMonitor
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
Definition: move_group_interface.cpp:2130
moveit::planning_interface::MoveGroupInterface::setGoalOrientationTolerance
void setGoalOrientationTolerance(double tolerance)
Set the orientation tolerance that is used for reaching the goal when moving to a pose.
Definition: move_group_interface.cpp:2120
moveit::planning_interface::MoveGroupInterface::setOrientationTarget
bool setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be the quaternion (x,...
Definition: move_group_interface.cpp:2052
moveit::planning_interface::MoveGroupInterface::Plan
The representation of a motion plan (as ROS messasges)
Definition: move_group_interface.h:195
moveit::planning_interface::MoveGroupInterface::setStartStateToCurrentState
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot's joint state publication.
Definition: move_group_interface.cpp:1678
moveit::planning_interface::MoveGroupInterface::getCurrentState
moveit::core::RobotStatePtr getCurrentState(double wait=1) const
Get the current state of the robot within the duration specified by wait.
Definition: move_group_interface.cpp:2242
moveit::planning_interface::MoveGroupInterface
Client class to conveniently use the ROS interfaces provided by the move_group node.
Definition: move_group_interface.h:165
moveit::planning_interface::MoveGroupInterface::allowReplanning
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
Definition: move_group_interface.cpp:2278
moveit::planning_interface::MoveGroupInterface::getVariableCount
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
Definition: move_group_interface.cpp:2237
moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
Definition: move_group_interface.h:169
moveit::planning_interface::MoveGroupInterface::move
MoveItErrorCode move()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
Definition: move_group_interface.cpp:1560
moveit::planning_interface::MoveGroupInterface::asyncExecute
MoveItErrorCode asyncExecute(const Plan &plan)
Given a plan, execute it without waiting for completion.
Definition: move_group_interface.cpp:1565
moveit::planning_interface::MoveGroupInterface::forgetJointValues
void forgetJointValues(const std::string &name)
Forget the joint values remembered under name.
Definition: move_group_interface.cpp:2254
moveit::planning_interface::MoveGroupInterface::getPlanningFrame
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
Definition: move_group_interface.cpp:2377
moveit::planning_interface::MoveGroupInterface::setLookAroundAttempts
void setLookAroundAttempts(int32_t attempts)
How often is the system allowed to move the camera to update environment model when looking.
Definition: move_group_interface.cpp:2265
moveit::planning_interface::MoveGroupInterface::setPlannerId
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
Definition: move_group_interface.cpp:1525
moveit::planning_interface::MoveGroupInterface::setGoalJointTolerance
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
Definition: move_group_interface.cpp:2110
moveit::planning_interface::MoveGroupInterface::getCurrentRPY
std::vector< double > getCurrentRPY(const std::string &end_effector_link="") const
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (t...
Definition: move_group_interface.cpp:2200
moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
Definition: move_group_interface.cpp:2382
buffer.h
moveit::planning_interface::MoveGroupInterface::setReplanDelay
void setReplanDelay(double delay)
Sleep this duration between replanning attempts (in walltime seconds)
Definition: move_group_interface.cpp:2297
moveit::core::RobotState
moveit::planning_interface::MoveGroupInterface::setRandomTarget
void setRandomTarget()
Set the joint state goal to a random joint configuration.
Definition: move_group_interface.cpp:1683
robot_state.h
moveit::planning_interface::MoveGroupInterface::getCurrentJointValues
std::vector< double > getCurrentJointValues() const
Get the current joint values for the joints planned for by this instance (see getJoints())
Definition: move_group_interface.cpp:2135
moveit::planning_interface::MoveGroupInterface::setPlanningTime
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
Definition: move_group_interface.cpp:2361
moveit::planning_interface::MoveGroupInterface::clearPathConstraints
void clearPathConstraints()
Specify that no path constraints are to be used. This removes any path constraints set in previous ca...
Definition: move_group_interface.cpp:2330
moveit::planning_interface::MoveGroupInterface::planGraspsAndPick
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
Definition: move_group_interface.cpp:1615
moveit::planning_interface::MoveGroupInterface::getEndEffectorLink
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
Definition: move_group_interface.cpp:1873
moveit::planning_interface::MoveGroupInterface::clearPoseTargets
void clearPoseTargets()
Forget any poses specified for all end-effectors.
Definition: move_group_interface.cpp:1905
moveit::planning_interface::MoveGroupInterface::getPoseTargets
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1984
moveit::planning_interface::MoveGroupInterface::getTargetRobotState
const moveit::core::RobotState & getTargetRobotState() const
Definition: move_group_interface.cpp:1868
moveit::planning_interface::MoveGroupInterface::rememberJointValues
void rememberJointValues(const std::string &name)
Remember the current joint values (of the robot being monitored) under name. These can be used by set...
Definition: move_group_interface.cpp:2125
moveit::planning_interface::MoveGroupInterface::setPlannerParams
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool bReplace=false)
Set the planner parameters for given group and planner_id.
Definition: move_group_interface.cpp:1499
moveit::planning_interface::MoveGroupInterface::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(Plan)
moveit::planning_interface::MoveGroupInterface::setGoalPositionTolerance
void setGoalPositionTolerance(double tolerance)
Set the position tolerance that is used for reaching the goal when moving to a pose.
Definition: move_group_interface.cpp:2115
moveit::planning_interface::MoveGroupInterface::remembered_joint_values_
std::map< std::string, std::vector< double > > remembered_joint_values_
Definition: move_group_interface.h:1092
moveit::planning_interface::MoveGroupInterface::Options::Options
Options(std::string group_name, std::string desc=ROBOT_DESCRIPTION, const ros::NodeHandle &node_handle=ros::NodeHandle())
Definition: move_group_interface.h:174
moveit::planning_interface::MoveGroupInterface::computeCartesianPath
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::MoveItErrorCodes *error_code=nullptr)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters...
Definition: move_group_interface.cpp:1630
moveit::planning_interface::MoveGroupInterface::allowLooking
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
Definition: move_group_interface.cpp:2259
moveit::planning_interface::MoveGroupInterface::setReplanAttempts
void setReplanAttempts(int32_t attempts)
Maximum number of replanning attempts.
Definition: move_group_interface.cpp:2284
moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner of the current planning pipeline for the given group (or the pipeline's defau...
Definition: move_group_interface.cpp:1520
moveit::planning_interface::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
actionlib::SimpleActionClient
moveit::planning_interface::MoveGroupInterface::getPoseReferenceFrame
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
Definition: move_group_interface.cpp:2083
moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase
void setConstraintsDatabase(const std::string &host, unsigned int port)
Specify where the database server that holds known constraints resides.
Definition: move_group_interface.cpp:2350
moveit::planning_interface::MoveGroupInterface::setPoseReferenceFrame
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
Definition: move_group_interface.cpp:2078
moveit::planning_interface::MoveGroupInterface::getRobotModel
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
Definition: move_group_interface.cpp:1473
moveit::planning_interface::MoveGroupInterface::getRandomPose
geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link="") const
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (th...
Definition: move_group_interface.cpp:2151
moveit::planning_interface::MoveGroupInterface::Options::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
Definition: move_group_interface.h:187
moveit::planning_interface::MoveGroupInterface::execute
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion.
Definition: move_group_interface.cpp:1575
moveit::planning_interface::MoveGroupInterface::getNodeHandle
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
Definition: move_group_interface.cpp:1478
simple_action_client.h
moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface
~MoveGroupInterface()
Definition: move_group_interface.cpp:1437
moveit::planning_interface::MoveGroupInterface::setMaxVelocityScalingFactor
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,...
Definition: move_group_interface.cpp:1540
moveit::planning_interface::MoveGroupInterface::getName
const std::string & getName() const
Get the name of the group this instance operates on.
Definition: move_group_interface.cpp:1461
moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFactor
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0...
Definition: move_group_interface.cpp:1545
moveit::planning_interface::MoveGroupInterface::Options::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_interface.h:189
moveit::planning_interface::MoveGroupInterface::setGoalTolerance
void setGoalTolerance(double tolerance)
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance fo...
Definition: move_group_interface.cpp:2103
moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget
bool setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
Definition: move_group_interface.cpp:1844
moveit::planning_interface::MoveGroupInterface::setEndEffectorLink
bool setEndEffectorLink(const std::string &end_effector_link)
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose tar...
Definition: move_group_interface.cpp:1883
moveit::planning_interface::MoveGroupInterface::setPlanningPipelineId
void setPlanningPipelineId(const std::string &pipeline_id)
Specify a planning pipeline to be used for further planning.
Definition: move_group_interface.cpp:1510
moveit::planning_interface::MoveGroupInterface::setRPYTarget
bool setRPYTarget(double roll, double pitch, double yaw, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians.
Definition: move_group_interface.cpp:2029
moveit::planning_interface::MoveGroupInterface::posesToPlaceLocations
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses) const
Convert a vector of PoseStamped to a vector of PlaceLocation.
Definition: move_group_interface.cpp:1605
moveit::planning_interface::MoveItErrorCode
Definition: move_group_interface.h:129
moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
Definition: move_group_interface.cpp:2340
moveit::planning_interface::MoveGroupInterface::setPathConstraints
bool setPathConstraints(const std::string &constraint)
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo databa...
Definition: move_group_interface.cpp:2320
moveit::planning_interface::MoveGroupInterface::stop
void stop()
Stop any trajectory execution, if one is active.
Definition: move_group_interface.cpp:1657
moveit::planning_interface::MoveGroupInterface::Options::robot_description_
std::string robot_description_
The robot description parameter name (if different from default)
Definition: move_group_interface.h:184
moveit::planning_interface::MoveGroupInterface::getPoseTarget
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1978
moveit::planning_interface::MoveGroupInterface::getGoalJointTolerance
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
Definition: move_group_interface.cpp:2088
moveit::planning_interface::MoveGroupInterface::getRandomJointValues
std::vector< double > getRandomJointValues() const
Get random joint values for the joints planned for by this instance (see getJoints())
Definition: move_group_interface.cpp:2144
moveit::planning_interface::MoveGroupInterface::getPlannerId
const std::string & getPlannerId() const
Get the current planner_id.
Definition: move_group_interface.cpp:1530
moveit::planning_interface::MoveGroupInterface::getJointValueTarget
const moveit::core::RobotState & getJointValueTarget() const
Get the currently set joint state goal, replaced by private getTargetRobotState()
Definition: move_group_interface.cpp:1863
moveit::planning_interface::MoveGroupInterface::operator=
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints
void clearTrajectoryConstraints()
Definition: move_group_interface.cpp:2345
moveit::planning_interface::MoveGroupInterface::constructPlaceGoal
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > locations, bool plan_only) const
Build a PlaceGoal for an object named object and store it in goal_out.
Definition: move_group_interface.cpp:1597
moveit::planning_interface::MoveGroupInterface::getPlanningPipelineId
const std::string & getPlanningPipelineId() const
Get the current planning_pipeline_id.
Definition: move_group_interface.cpp:1515
moveit
moveit::planning_interface::MoveGroupInterface::getNamedTargets
const std::vector< std::string > & getNamedTargets() const
Get the names of the named robot states available as targets, both either remembered states or defaul...
Definition: move_group_interface.cpp:1466
moveit::planning_interface::MoveGroupInterface::getInterfaceDescriptions
bool getInterfaceDescriptions(std::vector< moveit_msgs::PlannerInterfaceDescription > &desc) const
Get the descriptions of all planning plugins loaded by the action server.
Definition: move_group_interface.cpp:1488
moveit::planning_interface::MoveGroupInterface::setSupportSurfaceName
void setSupportSurfaceName(const std::string &name)
For pick/place operations, the name of the support surface is used to specify the fact that attached ...
Definition: move_group_interface.cpp:2372
class_forward.h
moveit::planning_interface::MoveGroupInterface::clearPoseTarget
void clearPoseTarget(const std::string &end_effector_link="")
Forget pose(s) specified for end_effector_link.
Definition: move_group_interface.cpp:1900
moveit::planning_interface::MoveGroupInterface::setNumPlanningAttempts
void setNumPlanningAttempts(unsigned int num_planning_attempts)
Set the number of times the motion plan is to be computed from scratch before the shortest solution i...
Definition: move_group_interface.cpp:1535
std
planning_interface
moveit::planning_interface::MoveGroupInterface::setPositionTarget
bool setPositionTarget(double x, double y, double z, const std::string &end_effector_link="")
Set the goal position of the end-effector end_effector_link to be (x, y, z).
Definition: move_group_interface.cpp:2004
moveit::planning_interface::MoveGroupInterface::getInterfaceDescription
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc) const
Get the description of the default planning plugin loaded by the action server.
Definition: move_group_interface.cpp:1483
moveit::planning_interface::MoveGroupInterface::Plan::trajectory_
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_)
Definition: move_group_interface.h:201
moveit::planning_interface::MoveGroupInterface::Plan::planning_time_
double planning_time_
The amount of time it took to generate the plan.
Definition: move_group_interface.h:204
moveit::planning_interface::MoveGroupInterface::attachObject
bool attachObject(const std::string &object, const std::string &link="")
Given the name of an object in the planning scene, make the object attached to a link of the robot....
Definition: move_group_interface.cpp:2387
moveit::planning_interface::MoveGroupInterface::setWorkspace
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Specify the workspace bounding box. The box is specified in the planning frame (i....
Definition: move_group_interface.cpp:2355
moveit::planning_interface::MoveGroupInterface::pick
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
Definition: move_group_interface.h:893
moveit::planning_interface::MoveGroupInterface::impl_
MoveGroupInterfaceImpl * impl_
Definition: move_group_interface.h:1093
moveit::planning_interface::MoveItErrorCode::operator==
bool operator==(const int c) const
Definition: move_group_interface.h:181
moveit::planning_interface::MoveGroupInterface::getPathConstraints
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
Definition: move_group_interface.cpp:2315
moveit::planning_interface::MoveGroupInterface::getPlanningTime
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
Definition: move_group_interface.cpp:2367
moveit::planning_interface::MoveGroupInterface::MoveGroupInterface
MoveGroupInterface(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
Definition: move_group_interface.cpp:1424
moveit::planning_interface::MoveGroupInterface::getActiveJoints
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
Definition: move_group_interface.cpp:2227
moveit::planning_interface::MoveGroupInterface::getLinkNames
const std::vector< std::string > & getLinkNames() const
Get vector of names of links available in move group.
Definition: move_group_interface.cpp:1694
moveit::planning_interface::MoveGroupInterface::getJointNames
const std::vector< std::string > & getJointNames() const
Get vector of names of joints available in move group.
Definition: move_group_interface.cpp:1689
moveit::planning_interface::MoveGroupInterface::setPoseTargets
bool setPoseTargets(const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.
Definition: move_group_interface.cpp:1934
moveit::planning_interface::MoveGroupInterface::setNamedTarget
bool setNamedTarget(const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or,...
Definition: move_group_interface.cpp:1719
moveit::planning_interface::MoveGroupInterface::getRememberedJointValues
const std::map< std::string, std::vector< double > > & getRememberedJointValues() const
Get the currently remembered map of names to joint values.
Definition: move_group_interface.h:1041
moveit::planning_interface::MoveGroupInterface::getGoalOrientationTolerance
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll,...
Definition: move_group_interface.cpp:2098
ros::WallDuration
moveit::planning_interface::MoveItErrorCode::MoveItErrorCode
MoveItErrorCode()
Definition: move_group_interface.h:165
moveit::planning_interface::MoveGroupInterface::Plan::start_state_
moveit_msgs::RobotState start_state_
The full starting state used for planning.
Definition: move_group_interface.h:198
moveit::planning_interface::MoveGroupInterface::getNamedTargetValues
std::map< std::string, double > getNamedTargetValues(const std::string &name) const
Get the joint angles for targets specified by name.
Definition: move_group_interface.cpp:1699
moveit::planning_interface::MoveGroupInterface::asyncMove
MoveItErrorCode asyncMove()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
Definition: move_group_interface.cpp:1550
moveit::planning_interface::MoveGroupInterface::constructMotionPlanRequest
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and sto...
Definition: move_group_interface.cpp:2403
ros::Duration
moveit::planning_interface::MoveGroupInterface::getCurrentPose
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="") const
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value...
Definition: move_group_interface.cpp:2176
moveit::planning_interface::MoveGroupInterface::detachObject
bool detachObject(const std::string &name="")
Detach an object. name specifies the name of the object attached to this group, or the name of the li...
Definition: move_group_interface.cpp:2398
moveit::planning_interface::MoveGroupInterface::setEndEffector
bool setEndEffector(const std::string &eef_name)
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the...
Definition: move_group_interface.cpp:1892
moveit::planning_interface::MoveItErrorCode::operator!=
bool operator!=(const int c) const
Definition: move_group_interface.h:185
moveit::planning_interface::MoveGroupInterface::Options::group_name_
std::string group_name_
The group to construct the class instance for.
Definition: move_group_interface.h:181
ros::NodeHandle
moveit::planning_interface::MoveGroupInterface::constructPickupGoal
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > grasps, bool plan_only) const
Build a PickupGoal for an object named object and store it in goal_out.
Definition: move_group_interface.cpp:1590
moveit::planning_interface::MoveGroupInterface::getGoalPositionTolerance
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
Definition: move_group_interface.cpp:2093
test_cleanup.group
group
Definition: test_cleanup.py:8


planning_interface
Author(s): Ioan Sucan
autogenerated on Fri Dec 3 2021 03:24:37