moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
43 #include <moveit_msgs/msg/robot_trajectory.hpp>
44 #include <moveit_msgs/msg/robot_state.hpp>
45 #include <moveit_msgs/msg/planner_interface_description.hpp>
46 #include <moveit_msgs/msg/constraints.hpp>
47 #include <moveit_msgs/msg/grasp.hpp>
48 // #include <moveit_msgs/msg/place_location.hpp>
49 
50 // #include <moveit_msgs/action/pickup.hpp>
51 // #include <moveit_msgs/action/place.hpp>
52 #include <moveit_msgs/action/move_group.hpp>
53 #include <moveit_msgs/action/execute_trajectory.hpp>
54 
55 #include <moveit_msgs/msg/motion_plan_request.hpp>
56 #include <geometry_msgs/msg/pose_stamped.hpp>
57 
58 #include <rclcpp_action/rclcpp_action.hpp>
59 
60 #include <memory>
61 #include <utility>
62 #include <tf2_ros/buffer.h>
63 
64 #include <moveit_move_group_interface_export.h>
65 
66 namespace moveit
67 {
69 namespace planning_interface
70 {
71 using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
72 
73 MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc
74 
80 class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
81 {
82 public:
84  static const std::string ROBOT_DESCRIPTION;
85 
87  struct Options
88  {
89  Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "")
90  : group_name_(std::move(group_name))
91  , robot_description_(std::move(desc))
92  , move_group_namespace_(std::move(move_group_namespace))
93  {
94  }
95 
97  std::string group_name_;
98 
100  std::string robot_description_;
101 
103  moveit::core::RobotModelConstPtr robot_model_;
104 
107  };
108 
110 
112  struct Plan
113  {
115  moveit_msgs::msg::RobotState start_state_;
116 
118  moveit_msgs::msg::RobotTrajectory trajectory_;
119 
122  };
123 
134  MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt,
135  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
136  const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1));
137 
145  MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group,
146  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
147  const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1));
148 
150 
158 
159  MoveGroupInterface(MoveGroupInterface&& other) noexcept;
160  MoveGroupInterface& operator=(MoveGroupInterface&& other) noexcept;
162  const std::string& getName() const;
163 
166  const std::vector<std::string>& getNamedTargets() const;
167 
169  moveit::core::RobotModelConstPtr getRobotModel() const;
170 
172  rclcpp::Node::SharedPtr getNodeHandle();
173 
175  const std::string& getPlanningFrame() const;
176 
178  const std::vector<std::string>& getJointModelGroupNames() const;
179 
181  const std::vector<std::string>& getJointNames() const;
182 
184  const std::vector<std::string>& getLinkNames() const;
185 
187  std::map<std::string, double> getNamedTargetValues(const std::string& name) const;
188 
190  const std::vector<std::string>& getActiveJoints() const;
191 
193  const std::vector<std::string>& getJoints() const;
194 
197  unsigned int getVariableCount() const;
198 
200  bool getInterfaceDescriptions(std::vector<moveit_msgs::msg::PlannerInterfaceDescription>& desc) const;
201 
203  bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc) const;
204 
206  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id,
207  const std::string& group = "") const;
208 
210  void setPlannerParams(const std::string& planner_id, const std::string& group,
211  const std::map<std::string, std::string>& params, bool bReplace = false);
212 
213  std::string getDefaultPlanningPipelineId() const;
214 
216  void setPlanningPipelineId(const std::string& pipeline_id);
217 
219  const std::string& getPlanningPipelineId() const;
220 
222  std::string getDefaultPlannerId(const std::string& group = "") const;
223 
225  void setPlannerId(const std::string& planner_id);
226 
228  const std::string& getPlannerId() const;
229 
231  void setPlanningTime(double seconds);
232 
235  void setNumPlanningAttempts(unsigned int num_planning_attempts);
236 
242  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
243 
249  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
250 
252  double getPlanningTime() const;
253 
256  double getGoalJointTolerance() const;
257 
260  double getGoalPositionTolerance() const;
261 
264  double getGoalOrientationTolerance() const;
265 
272  void setGoalTolerance(double tolerance);
273 
276  void setGoalJointTolerance(double tolerance);
277 
279  void setGoalPositionTolerance(double tolerance);
280 
282  void setGoalOrientationTolerance(double tolerance);
283 
288  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
289 
292  void setStartState(const moveit_msgs::msg::RobotState& start_state);
293 
296  void setStartState(const moveit::core::RobotState& start_state);
297 
299  void setStartStateToCurrentState();
300 
303  void setSupportSurfaceName(const std::string& name);
304 
334  bool setJointValueTarget(const std::vector<double>& group_variable_values);
335 
351  bool setJointValueTarget(const std::map<std::string, double>& variable_values);
352 
368  bool setJointValueTarget(const std::vector<std::string>& variable_names, const std::vector<double>& variable_values);
369 
379  bool setJointValueTarget(const moveit::core::RobotState& robot_state);
380 
392  bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
393 
405  bool setJointValueTarget(const std::string& joint_name, double value);
406 
417  bool setJointValueTarget(const sensor_msgs::msg::JointState& state);
418 
430  bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link = "");
431 
443  bool setJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose, const std::string& end_effector_link = "");
444 
456  bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
457 
468  bool setApproximateJointValueTarget(const geometry_msgs::msg::Pose& eef_pose,
469  const std::string& end_effector_link = "");
470 
481  bool setApproximateJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose,
482  const std::string& end_effector_link = "");
483 
494  bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
495 
500  void setRandomTarget();
501 
504  bool setNamedTarget(const std::string& name);
505 
507  void getJointValueTarget(std::vector<double>& group_variable_values) const;
508 
510  [[deprecated]] const moveit::core::RobotState& getJointValueTarget() const;
511 
533  bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
534 
542  bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
543 
552  bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
553 
561  bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
562 
570  bool setPoseTarget(const geometry_msgs::msg::Pose& target, const std::string& end_effector_link = "");
571 
579  bool setPoseTarget(const geometry_msgs::msg::PoseStamped& target, const std::string& end_effector_link = "");
580 
599  bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
600 
619  bool setPoseTargets(const std::vector<geometry_msgs::msg::Pose>& target, const std::string& end_effector_link = "");
620 
639  bool setPoseTargets(const std::vector<geometry_msgs::msg::PoseStamped>& target,
640  const std::string& end_effector_link = "");
641 
643  void setPoseReferenceFrame(const std::string& pose_reference_frame);
644 
648  bool setEndEffectorLink(const std::string& end_effector_link);
649 
652  bool setEndEffector(const std::string& eef_name);
653 
655  void clearPoseTarget(const std::string& end_effector_link = "");
656 
658  void clearPoseTargets();
659 
666  const geometry_msgs::msg::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
667 
673  const std::vector<geometry_msgs::msg::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
674 
680  const std::string& getEndEffectorLink() const;
681 
687  const std::string& getEndEffector() const;
688 
691  const std::string& getPoseReferenceFrame() const;
692 
703  moveit::core::MoveItErrorCode asyncMove();
704 
709  rclcpp_action::Client<moveit_msgs::action::MoveGroup>& getMoveGroupClient() const;
710 
716 
721 
723  moveit::core::MoveItErrorCode asyncExecute(const Plan& plan);
724 
726  moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory);
727 
729  moveit::core::MoveItErrorCode execute(const Plan& plan);
730 
732  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory);
733 
743  double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
744  double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
745  bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
746 
759  double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
760  double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
761  const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true,
762  moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
763 
765  void stop();
766 
768  void allowReplanning(bool flag);
769 
771  void setReplanAttempts(int32_t attempts);
772 
774  void setReplanDelay(double delay);
775 
778  void allowLooking(bool flag);
779 
781  void setLookAroundAttempts(int32_t attempts);
782 
785  void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);
786 
789  // moveit_msgs::action::Pickup::Goal constructPickupGoal(const std::string& object,
790  // std::vector<moveit_msgs::msg::Grasp> grasps,
791  // bool plan_only) const;
792  //
793  // /** \brief Build a PlaceGoal for an object named \e object and store it in \e goal_out */
794  // moveit_msgs::action::Place::Goal constructPlaceGoal(const std::string& object,
795  // std::vector<moveit_msgs::msg::PlaceLocation> locations,
796  // bool plan_only) const;
797  //
798  // /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */
799  // std::vector<moveit_msgs::msg::PlaceLocation>
800  // posesToPlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& poses) const;
801 
812  // moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false)
813  // {
814  // return pick(constructPickupGoal(object, std::vector<moveit_msgs::msg::Grasp>(), plan_only));
815  // }
816  //
818  // moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false)
819  // {
820  // return pick(constructPickupGoal(object, { grasp }, plan_only));
821  // }
822  //
824  // moveit::core::MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::msg::Grasp> grasps, bool
825  // plan_only = false)
826  // {
827  // return pick(constructPickupGoal(object, std::move(grasps), plan_only));
828  // }
829 
835  // moveit::core::MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal);
836 
840  // moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
841 
844  // moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false);
845 
847  // moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false)
848  // {
849  // return place(constructPlaceGoal(object, std::vector<moveit_msgs::msg::PlaceLocation>(), plan_only));
850  // }
851 
853  // moveit::core::MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations,
854  // bool plan_only = false)
855  // {
856  // return place(constructPlaceGoal(object, std::move(locations), plan_only));
857  // }
858 
860  // moveit::core::MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::msg::PoseStamped>& poses,
861  // bool plan_only = false)
862  // {
863  // return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
864  // }
865 
867  // moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool
868  // plan_only = false)
869  // {
870  // return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
871  // }
872 
878  // moveit::core::MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal);
879 
886  bool attachObject(const std::string& object, const std::string& link = "");
887 
896  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
897 
903  bool detachObject(const std::string& name = "");
904 
917  bool startStateMonitor(double wait = 1.0);
918 
920  std::vector<double> getCurrentJointValues() const;
921 
923  moveit::core::RobotStatePtr getCurrentState(double wait = 1) const;
924 
928  geometry_msgs::msg::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const;
929 
933  std::vector<double> getCurrentRPY(const std::string& end_effector_link = "") const;
934 
936  std::vector<double> getRandomJointValues() const;
937 
941  geometry_msgs::msg::PoseStamped getRandomPose(const std::string& end_effector_link = "") const;
942 
954  void rememberJointValues(const std::string& name);
955 
960  void rememberJointValues(const std::string& name, const std::vector<double>& values);
961 
963  const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
964  {
965  return remembered_joint_values_;
966  }
967 
969  void forgetJointValues(const std::string& name);
970 
979  void setConstraintsDatabase(const std::string& host, unsigned int port);
980 
982  std::vector<std::string> getKnownConstraints() const;
983 
987  moveit_msgs::msg::Constraints getPathConstraints() const;
988 
992  bool setPathConstraints(const std::string& constraint);
993 
997  void setPathConstraints(const moveit_msgs::msg::Constraints& constraint);
998 
1001  void clearPathConstraints();
1002 
1003  moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const;
1004  void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& constraint);
1005  void clearTrajectoryConstraints();
1006 
1009 protected:
1011  const moveit::core::RobotState& getTargetRobotState() const;
1012 
1013 private:
1014  std::map<std::string, std::vector<double> > remembered_joint_values_;
1015  class MoveGroupInterfaceImpl;
1016  MoveGroupInterfaceImpl* impl_;
1017 };
1018 } // namespace planning_interface
1019 } // namespace moveit
a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Client class to conveniently use the ROS interfaces provided by the move_group node.
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
MoveGroupInterface(const MoveGroupInterface &)=delete
This class owns unique resources (e.g. action clients, threads) and its not very meaningful to copy....
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
const std::map< std::string, std::vector< double > > & getRememberedJointValues() const
Get the currently remembered map of names to joint values.
rclcpp::Node::SharedPtr getNodeHandle()
Get the ROS node handle of this instance operates on.
void attachObject()
Definition: demo.cpp:104
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
moveit::core::MoveItErrorCode MoveItErrorCode
Main namespace for MoveIt.
Definition: exceptions.h:43
w
Definition: pick.py:67
x
Definition: pick.py:64
y
Definition: pick.py:65
z
Definition: pick.py:66
Definition: plan.py:1
This namespace includes the base class for MoveIt planners.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
name
Definition: setup.py:7
Specification of options to use when constructing the MoveGroupInterface class.
std::string group_name_
The group to construct the class instance for.
Options(std::string group_name, std::string desc=ROBOT_DESCRIPTION, std::string move_group_namespace="")
std::string move_group_namespace_
The namespace for the move group node.
moveit::core::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
std::string robot_description_
The robot description parameter name (if different from default)
The representation of a motion plan (as ROS messasges)
moveit_msgs::msg::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_)
moveit_msgs::msg::RobotState start_state_
The full starting state used for planning.
double planning_time_
The amount of time it took to generate the plan.