move_group_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, SRI International
5  * Copyright (c) 2013, Ioan A. Sucan
6  * Copyright (c) 2012, Willow Garage, Inc.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Ioan Sucan, Sachin Chitta */
38 
39 #include <stdexcept>
40 #include <sstream>
41 #include <memory>
53 #include <moveit_msgs/PickupAction.h>
54 #include <moveit_msgs/ExecuteTrajectoryAction.h>
55 #include <moveit_msgs/PlaceAction.h>
56 #include <moveit_msgs/ExecuteKnownTrajectory.h>
57 #include <moveit_msgs/QueryPlannerInterfaces.h>
58 #include <moveit_msgs/GetCartesianPath.h>
59 #include <moveit_msgs/GraspPlanning.h>
60 #include <moveit_msgs/GetPlannerParams.h>
61 #include <moveit_msgs/SetPlannerParams.h>
62 
63 #include <std_msgs/String.h>
64 #include <geometry_msgs/TransformStamped.h>
65 #include <tf2/utils.h>
66 #include <tf2_eigen/tf2_eigen.h>
68 #include <ros/console.h>
69 #include <ros/ros.h>
70 
71 namespace moveit
72 {
73 namespace planning_interface
74 {
75 const std::string MoveGroupInterface::ROBOT_DESCRIPTION =
76  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
77 
78 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps
79 
80 const std::string LOGNAME = "move_group_interface";
81 
82 namespace
83 {
84 enum ActiveTargetType
85 {
86  JOINT,
87  POSE,
88  POSITION,
89  ORIENTATION
90 };
91 }
92 
93 class MoveGroupInterface::MoveGroupInterfaceImpl
94 {
95 public:
96  MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
97  const ros::WallDuration& wait_for_servers)
99  {
100  robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
101  if (!getRobotModel())
102  {
103  std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
104  "parameter server.";
106  throw std::runtime_error(error);
107  }
108 
109  if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
110  {
111  std::string error = "Group '" + opt.group_name_ + "' was not found.";
113  throw std::runtime_error(error);
114  }
115 
116  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
117 
119  joint_state_target_->setToDefaultValues();
120  active_target_ = JOINT;
121  can_look_ = false;
122  can_replan_ = false;
123  replan_delay_ = 2.0;
124  goal_joint_tolerance_ = 1e-4;
125  goal_position_tolerance_ = 1e-4; // 0.1 mm
126  goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
129  node_handle_.param<double>("robot_description_planning/joint_limits/default_velocity_scaling_factor",
131  node_handle_.param<double>("robot_description_planning/joint_limits/default_acceleration_scaling_factor",
134 
137  pose_reference_frame_ = getRobotModel()->getModelFrame();
138 
141  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
143 
145 
146  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
147  if (wait_for_servers == ros::WallDuration())
148  timeout_for_servers = ros::WallTime(); // wait for ever
149  double allotted_time = wait_for_servers.toSec();
150 
151  move_action_client_.reset(
153  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
154 
155  pick_action_client_.reset(
157  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
158 
159  place_action_client_.reset(
161  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
162 
165  waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
166 
168  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
173 
176 
178 
179  ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << opt.group_name_ << ".");
180  }
181 
182  template <typename T>
183  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) const
184  {
185  ROS_DEBUG_NAMED(LOGNAME, "Waiting for move_group action server (%s)...", name.c_str());
186 
187  // wait for the server (and spin as needed)
188  if (timeout == ros::WallTime()) // wait forever
189  {
190  while (node_handle_.ok() && !action->isServerConnected())
191  {
192  ros::WallDuration(0.001).sleep();
193  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
195  if (queue)
196  {
197  queue->callAvailable();
198  }
199  else // in case of nodelets and specific callback queue implementations
200  {
201  ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue "
202  "handling.");
203  }
204  }
205  }
206  else // wait with timeout
207  {
208  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
209  {
210  ros::WallDuration(0.001).sleep();
211  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
213  if (queue)
214  {
215  queue->callAvailable();
216  }
217  else // in case of nodelets and specific callback queue implementations
218  {
219  ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue "
220  "handling.");
221  }
222  }
223  }
224 
225  if (!action->isServerConnected())
226  {
227  std::stringstream error;
228  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
229  << "s)";
230  throw std::runtime_error(error.str());
231  }
232  else
233  {
234  ROS_DEBUG_NAMED(LOGNAME, "Connected to '%s'", name.c_str());
235  }
236  }
237 
239  {
241  constraints_init_thread_->join();
242  }
243 
244  const std::shared_ptr<tf2_ros::Buffer>& getTF() const
245  {
246  return tf_buffer_;
247  }
248 
249  const Options& getOptions() const
250  {
251  return opt_;
252  }
253 
254  const moveit::core::RobotModelConstPtr& getRobotModel() const
255  {
256  return robot_model_;
257  }
258 
260  {
261  return joint_model_group_;
262  }
263 
265  {
266  return *move_action_client_;
267  }
268 
269  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
270  {
271  moveit_msgs::QueryPlannerInterfaces::Request req;
272  moveit_msgs::QueryPlannerInterfaces::Response res;
273  if (query_service_.call(req, res))
274  if (!res.planner_interfaces.empty())
275  {
276  desc = res.planner_interfaces.front();
277  return true;
278  }
279  return false;
280  }
281 
282  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
283  {
284  moveit_msgs::GetPlannerParams::Request req;
285  moveit_msgs::GetPlannerParams::Response res;
286  req.planner_config = planner_id;
287  req.group = group;
288  std::map<std::string, std::string> result;
289  if (get_params_service_.call(req, res))
290  {
291  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
292  result[res.params.keys[i]] = res.params.values[i];
293  }
294  return result;
295  }
296 
297  void setPlannerParams(const std::string& planner_id, const std::string& group,
298  const std::map<std::string, std::string>& params, bool replace = false)
299  {
300  moveit_msgs::SetPlannerParams::Request req;
301  moveit_msgs::SetPlannerParams::Response res;
302  req.planner_config = planner_id;
303  req.group = group;
304  req.replace = replace;
305  for (const std::pair<const std::string, std::string>& param : params)
306  {
307  req.params.keys.push_back(param.first);
308  req.params.values.push_back(param.second);
309  }
310  set_params_service_.call(req, res);
311  }
312 
313  std::string getDefaultPlannerId(const std::string& group) const
314  {
315  std::stringstream param_name;
316  param_name << "move_group";
317  if (!group.empty())
318  param_name << "/" << group;
319  param_name << "/default_planner_config";
320 
321  std::string default_planner_config;
322  node_handle_.getParam(param_name.str(), default_planner_config);
323  return default_planner_config;
324  }
325 
326  void setPlannerId(const std::string& planner_id)
327  {
328  planner_id_ = planner_id;
329  }
330 
331  const std::string& getPlannerId() const
332  {
333  return planner_id_;
334  }
335 
336  void setNumPlanningAttempts(unsigned int num_planning_attempts)
337  {
338  num_planning_attempts_ = num_planning_attempts;
339  }
340 
341  void setMaxVelocityScalingFactor(double value)
342  {
343  setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1);
344  }
345 
346  void setMaxAccelerationScalingFactor(double value)
347  {
348  setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1);
349  }
350 
351  void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value)
352  {
353  if (target_value > 1.0)
354  {
355  ROS_WARN_NAMED(LOGNAME, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value);
356  variable = 1.0;
357  }
358  else if (target_value <= 0.0)
359  {
360  node_handle_.param<double>(std::string("robot_description_planning/default_") + factor_name, variable,
361  fallback_value);
362  if (target_value < 0.0)
363  {
364  ROS_WARN_NAMED(LOGNAME, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable);
365  }
366  }
367  else
368  {
369  variable = target_value;
370  }
371  }
372 
374  {
375  return *joint_state_target_;
376  }
377 
379  {
380  return *joint_state_target_;
381  }
382 
383  void setStartState(const moveit::core::RobotState& start_state)
384  {
385  considered_start_state_.reset(new moveit::core::RobotState(start_state));
386  }
387 
389  {
390  considered_start_state_.reset();
391  }
392 
393  moveit::core::RobotStatePtr getStartState()
394  {
397  else
398  {
399  moveit::core::RobotStatePtr s;
400  getCurrentState(s);
401  return s;
402  }
403  }
404 
405  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
406  const std::string& frame, bool approx)
407  {
408  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
409  // this only works if we have an end-effector
410  if (!eef.empty())
411  {
412  // first we set the goal to be the same as the start state
413  moveit::core::RobotStatePtr c = getStartState();
414  if (c)
415  {
416  setTargetType(JOINT);
417  c->enforceBounds();
418  getTargetRobotState() = *c;
419  if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance()))
420  return false;
421  }
422  else
423  return false;
424 
425  // we may need to do approximate IK
428 
429  // if no frame transforms are needed, call IK directly
430  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
431  return getTargetRobotState().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0,
433  else
434  {
435  // transform the pose into the model frame, then do IK
436  bool frame_found;
437  const Eigen::Isometry3d& t = getTargetRobotState().getFrameTransform(frame, &frame_found);
438  if (frame_found)
439  {
440  Eigen::Isometry3d p;
441  tf2::fromMsg(eef_pose, p);
442  return getTargetRobotState().setFromIK(getJointModelGroup(), t * p, eef, 0.0,
444  }
445  else
446  {
447  ROS_ERROR_NAMED(LOGNAME, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
448  getRobotModel()->getModelFrame().c_str());
449  return false;
450  }
451  }
452  }
453  else
454  return false;
455  }
456 
457  void setEndEffectorLink(const std::string& end_effector)
458  {
459  end_effector_link_ = end_effector;
460  }
461 
462  void clearPoseTarget(const std::string& end_effector_link)
463  {
464  pose_targets_.erase(end_effector_link);
465  }
466 
467  void clearPoseTargets()
468  {
469  pose_targets_.clear();
470  }
471 
472  const std::string& getEndEffectorLink() const
473  {
474  return end_effector_link_;
475  }
476 
477  const std::string& getEndEffector() const
478  {
479  if (!end_effector_link_.empty())
480  {
481  const std::vector<std::string>& possible_eefs =
482  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
483  for (const std::string& possible_eef : possible_eefs)
484  if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_))
485  return possible_eef;
486  }
487  static std::string empty;
488  return empty;
489  }
490 
491  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
492  {
493  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
494  if (eef.empty())
495  {
496  ROS_ERROR_NAMED(LOGNAME, "No end-effector to set the pose for");
497  return false;
498  }
499  else
500  {
501  pose_targets_[eef] = poses;
502  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
503  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
504  for (geometry_msgs::PoseStamped& stored_pose : stored_poses)
505  stored_pose.header.stamp = ros::Time(0);
506  }
507  return true;
508  }
509 
510  bool hasPoseTarget(const std::string& end_effector_link) const
511  {
512  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
513  return pose_targets_.find(eef) != pose_targets_.end();
514  }
515 
516  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
517  {
518  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
519 
520  // if multiple pose targets are set, return the first one
521  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
522  if (jt != pose_targets_.end())
523  if (!jt->second.empty())
524  return jt->second.at(0);
525 
526  // or return an error
527  static const geometry_msgs::PoseStamped UNKNOWN;
528  ROS_ERROR_NAMED(LOGNAME, "Pose for end-effector '%s' not known.", eef.c_str());
529  return UNKNOWN;
530  }
531 
532  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
533  {
534  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
535 
536  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
537  if (jt != pose_targets_.end())
538  if (!jt->second.empty())
539  return jt->second;
540 
541  // or return an error
542  static const std::vector<geometry_msgs::PoseStamped> EMPTY;
543  ROS_ERROR_NAMED(LOGNAME, "Poses for end-effector '%s' are not known.", eef.c_str());
544  return EMPTY;
545  }
546 
547  void setPoseReferenceFrame(const std::string& pose_reference_frame)
548  {
549  pose_reference_frame_ = pose_reference_frame;
550  }
551 
552  void setSupportSurfaceName(const std::string& support_surface)
553  {
554  support_surface_ = support_surface;
555  }
556 
557  const std::string& getPoseReferenceFrame() const
558  {
560  }
561 
562  void setTargetType(ActiveTargetType type)
563  {
564  active_target_ = type;
565  }
566 
567  ActiveTargetType getTargetType() const
568  {
569  return active_target_;
570  }
571 
572  bool startStateMonitor(double wait)
573  {
575  {
576  ROS_ERROR_NAMED(LOGNAME, "Unable to monitor current robot state");
577  return false;
578  }
579 
580  // if needed, start the monitor and wait up to 1 second for a full robot state
581  if (!current_state_monitor_->isActive())
582  current_state_monitor_->startStateMonitor();
583 
584  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
585  return true;
586  }
587 
588  bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds = 1.0)
589  {
591  {
592  ROS_ERROR_NAMED(LOGNAME, "Unable to get current robot state");
593  return false;
594  }
595 
596  // if needed, start the monitor and wait up to 1 second for a full robot state
597  if (!current_state_monitor_->isActive())
598  current_state_monitor_->startStateMonitor();
599 
600  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
601  {
602  ROS_ERROR_NAMED(LOGNAME, "Failed to fetch current robot state");
603  return false;
604  }
605 
606  current_state = current_state_monitor_->getCurrentState();
607  return true;
608  }
609 
611  std::vector<moveit_msgs::PlaceLocation>
612  posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses) const
613  {
614  std::vector<moveit_msgs::PlaceLocation> locations;
615  for (const geometry_msgs::PoseStamped& pose : poses)
616  {
617  moveit_msgs::PlaceLocation location;
618  location.pre_place_approach.direction.vector.z = -1.0;
619  location.post_place_retreat.direction.vector.x = -1.0;
620  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
621  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
622 
623  location.pre_place_approach.min_distance = 0.1;
624  location.pre_place_approach.desired_distance = 0.2;
625  location.post_place_retreat.min_distance = 0.0;
626  location.post_place_retreat.desired_distance = 0.2;
627  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
628 
629  location.place_pose = pose;
630  locations.push_back(location);
631  }
632  ROS_DEBUG_NAMED(LOGNAME, "Move group interface has %u place locations", (unsigned int)locations.size());
633  return locations;
634  }
635 
636  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
637  {
639  {
640  ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found");
641  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
642  }
643  if (!place_action_client_->isServerConnected())
644  {
645  ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected");
646  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
647  }
648 
649  place_action_client_->sendGoal(goal);
650  ROS_DEBUG_NAMED(LOGNAME, "Sent place goal with %d locations", (int)goal.place_locations.size());
651  if (!place_action_client_->waitForResult())
652  {
653  ROS_INFO_STREAM_NAMED(LOGNAME, "Place action returned early");
654  }
656  {
657  return MoveItErrorCode(place_action_client_->getResult()->error_code);
658  }
659  else
660  {
661  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": "
662  << place_action_client_->getState().getText());
663  return MoveItErrorCode(place_action_client_->getResult()->error_code);
664  }
665  }
666 
667  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
668  {
669  if (!pick_action_client_)
670  {
671  ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found");
672  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
673  }
674  if (!pick_action_client_->isServerConnected())
675  {
676  ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected");
677  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
678  }
679 
680  pick_action_client_->sendGoal(goal);
681  if (!pick_action_client_->waitForResult())
682  {
683  ROS_INFO_STREAM_NAMED(LOGNAME, "Pickup action returned early");
684  }
686  {
687  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
688  }
689  else
690  {
691  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": "
692  << pick_action_client_->getState().getText());
693  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
694  }
695  }
696 
697  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
698  {
699  if (object.empty())
700  {
701  return planGraspsAndPick(moveit_msgs::CollisionObject());
702  }
703 
705  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
706 
707  if (objects.empty())
708  {
710  "Asked for grasps for the object '" << object << "', but the object could not be found");
711  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
712  }
713 
714  return planGraspsAndPick(objects[object], plan_only);
715  }
716 
717  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
718  {
720  {
721  ROS_ERROR_STREAM_NAMED(LOGNAME, "Grasp planning service '"
723  << "' is not available."
724  " This has to be implemented and started separately.");
725  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
726  }
727 
728  moveit_msgs::GraspPlanning::Request request;
729  moveit_msgs::GraspPlanning::Response response;
730 
731  request.group_name = opt_.group_name_;
732  request.target = object;
733  request.support_surfaces.push_back(support_surface_);
734 
735  ROS_DEBUG_NAMED(LOGNAME, "Calling grasp planner...");
736  if (!plan_grasps_service_.call(request, response) ||
737  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
738  {
739  ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick.");
740  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
741  }
742 
743  return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
744  }
745 
746  MoveItErrorCode plan(Plan& plan)
747  {
748  if (!move_action_client_)
749  {
750  ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
751  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
752  }
753  if (!move_action_client_->isServerConnected())
754  {
755  ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
756  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
757  }
758 
759  moveit_msgs::MoveGroupGoal goal;
760  constructGoal(goal);
761  goal.planning_options.plan_only = true;
762  goal.planning_options.look_around = false;
763  goal.planning_options.replan = false;
764  goal.planning_options.planning_scene_diff.is_diff = true;
765  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
766 
767  move_action_client_->sendGoal(goal);
768  if (!move_action_client_->waitForResult())
769  {
770  ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early");
771  }
773  {
774  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
775  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
776  plan.planning_time_ = move_action_client_->getResult()->planning_time;
777  return MoveItErrorCode(move_action_client_->getResult()->error_code);
778  }
779  else
780  {
781  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": "
782  << move_action_client_->getState().getText());
783  return MoveItErrorCode(move_action_client_->getResult()->error_code);
784  }
785  }
786 
787  MoveItErrorCode move(bool wait)
788  {
789  if (!move_action_client_)
790  {
791  ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
792  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
793  }
794  if (!move_action_client_->isServerConnected())
795  {
796  ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
797  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
798  }
799 
800  moveit_msgs::MoveGroupGoal goal;
801  constructGoal(goal);
802  goal.planning_options.plan_only = false;
803  goal.planning_options.look_around = can_look_;
804  goal.planning_options.replan = can_replan_;
805  goal.planning_options.replan_delay = replan_delay_;
806  goal.planning_options.planning_scene_diff.is_diff = true;
807  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
808 
809  move_action_client_->sendGoal(goal);
810  if (!wait)
811  {
812  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
813  }
814 
815  if (!move_action_client_->waitForResult())
816  {
817  ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early");
818  }
819 
821  {
822  return MoveItErrorCode(move_action_client_->getResult()->error_code);
823  }
824  else
825  {
826  ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString()
827  << ": " << move_action_client_->getState().getText());
828  return MoveItErrorCode(move_action_client_->getResult()->error_code);
829  }
830  }
831 
832  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
833  {
835  {
836  ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found");
837  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
838  }
839  if (!execute_action_client_->isServerConnected())
840  {
841  ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected");
842  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
843  }
844 
845  moveit_msgs::ExecuteTrajectoryGoal goal;
846  goal.trajectory = trajectory;
847 
848  execute_action_client_->sendGoal(goal);
849  if (!wait)
850  {
851  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
852  }
853 
854  if (!execute_action_client_->waitForResult())
855  {
856  ROS_INFO_STREAM_NAMED(LOGNAME, "ExecuteTrajectory action returned early");
857  }
858 
860  {
861  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
862  }
863  else
864  {
866  << ": " << execute_action_client_->getState().getText());
867  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
868  }
869  }
870 
871  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
872  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
873  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
874  {
875  moveit_msgs::GetCartesianPath::Request req;
876  moveit_msgs::GetCartesianPath::Response res;
877 
880  else
881  req.start_state.is_diff = true;
882 
883  req.group_name = opt_.group_name_;
884  req.header.frame_id = getPoseReferenceFrame();
885  req.header.stamp = ros::Time::now();
886  req.waypoints = waypoints;
887  req.max_step = step;
888  req.jump_threshold = jump_threshold;
889  req.path_constraints = path_constraints;
890  req.avoid_collisions = avoid_collisions;
891  req.link_name = getEndEffectorLink();
892 
893  if (cartesian_path_service_.call(req, res))
894  {
895  error_code = res.error_code;
896  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
897  {
898  msg = res.solution;
899  return res.fraction;
900  }
901  else
902  return -1.0;
903  }
904  else
905  {
906  error_code.val = error_code.FAILURE;
907  return -1.0;
908  }
909  }
910 
911  void stop()
912  {
914  {
915  std_msgs::String event;
916  event.data = "stop";
918  }
919  }
920 
921  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
922  {
923  std::string l = link.empty() ? getEndEffectorLink() : link;
924  if (l.empty())
925  {
926  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
927  if (!links.empty())
928  l = links[0];
929  }
930  if (l.empty())
931  {
932  ROS_ERROR_NAMED(LOGNAME, "No known link to attach object '%s' to", object.c_str());
933  return false;
934  }
935  moveit_msgs::AttachedCollisionObject aco;
936  aco.object.id = object;
937  aco.link_name.swap(l);
938  if (touch_links.empty())
939  aco.touch_links.push_back(aco.link_name);
940  else
941  aco.touch_links = touch_links;
942  aco.object.operation = moveit_msgs::CollisionObject::ADD;
944  return true;
945  }
946 
947  bool detachObject(const std::string& name)
948  {
949  moveit_msgs::AttachedCollisionObject aco;
950  // if name is a link
951  if (!name.empty() && joint_model_group_->hasLinkModel(name))
952  aco.link_name = name;
953  else
954  aco.object.id = name;
955  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
956  if (aco.link_name.empty() && aco.object.id.empty())
957  {
958  // we only want to detach objects for this group
959  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
960  for (const std::string& lname : lnames)
961  {
962  aco.link_name = lname;
964  }
965  }
966  else
968  return true;
969  }
970 
971  double getGoalPositionTolerance() const
972  {
974  }
975 
976  double getGoalOrientationTolerance() const
977  {
979  }
980 
981  double getGoalJointTolerance() const
982  {
983  return goal_joint_tolerance_;
984  }
985 
986  void setGoalJointTolerance(double tolerance)
987  {
989  }
990 
991  void setGoalPositionTolerance(double tolerance)
992  {
994  }
995 
996  void setGoalOrientationTolerance(double tolerance)
997  {
999  }
1000 
1001  void setPlanningTime(double seconds)
1002  {
1003  if (seconds > 0.0)
1005  }
1006 
1007  double getPlanningTime() const
1008  {
1009  return allowed_planning_time_;
1010  }
1011 
1012  void allowLooking(bool flag)
1013  {
1014  can_look_ = flag;
1015  ROS_INFO_NAMED(LOGNAME, "Looking around: %s", can_look_ ? "yes" : "no");
1016  }
1017 
1018  void allowReplanning(bool flag)
1019  {
1020  can_replan_ = flag;
1021  ROS_INFO_NAMED(LOGNAME, "Replanning: %s", can_replan_ ? "yes" : "no");
1022  }
1023 
1024  void setReplanningDelay(double delay)
1025  {
1026  if (delay >= 0.0)
1027  replan_delay_ = delay;
1028  }
1029 
1030  double getReplanningDelay() const
1031  {
1032  return replan_delay_;
1033  }
1034 
1035  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) const
1036  {
1037  request.group_name = opt_.group_name_;
1038  request.num_planning_attempts = num_planning_attempts_;
1039  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1040  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1041  request.allowed_planning_time = allowed_planning_time_;
1042  request.planner_id = planner_id_;
1043  request.workspace_parameters = workspace_parameters_;
1047  else
1048  request.start_state.is_diff = true;
1050  if (active_target_ == JOINT)
1051  {
1052  request.goal_constraints.resize(1);
1053  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1055  }
1056  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1057  {
1058  // find out how many goals are specified
1059  std::size_t goal_count = 0;
1060  for (const auto& pose_target : pose_targets_)
1061  goal_count = std::max(goal_count, pose_target.second.size());
1062 
1063  // start filling the goals;
1064  // each end effector has a number of possible poses (K) as valid goals
1065  // but there could be multiple end effectors specified, so we want each end effector
1066  // to reach the goal that corresponds to the goals of the other end effectors
1067  request.goal_constraints.resize(goal_count);
1068 
1069  for (const auto& pose_target : pose_targets_)
1070  {
1071  for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1072  {
1073  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1074  pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1075  if (active_target_ == ORIENTATION)
1076  c.position_constraints.clear();
1077  if (active_target_ == POSITION)
1078  c.orientation_constraints.clear();
1079  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1080  }
1081  }
1082  }
1083  else
1084  ROS_ERROR_NAMED(LOGNAME, "Unable to construct MotionPlanRequest representation");
1085 
1087  request.path_constraints = *path_constraints_;
1089  request.trajectory_constraints = *trajectory_constraints_;
1090  }
1091 
1092  void constructGoal(moveit_msgs::MoveGroupGoal& goal) const
1093  {
1094  constructMotionPlanRequest(goal.request);
1095  }
1096 
1097  moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp>&& grasps,
1098  bool plan_only = false) const
1099  {
1100  moveit_msgs::PickupGoal goal;
1101  goal.target_name = object;
1102  goal.group_name = opt_.group_name_;
1103  goal.end_effector = getEndEffector();
1104  goal.support_surface_name = support_surface_;
1105  goal.possible_grasps = std::move(grasps);
1106  if (!support_surface_.empty())
1107  goal.allow_gripper_support_collision = true;
1108 
1109  if (path_constraints_)
1110  goal.path_constraints = *path_constraints_;
1111 
1112  goal.planner_id = planner_id_;
1113  goal.allowed_planning_time = allowed_planning_time_;
1114 
1115  goal.planning_options.plan_only = plan_only;
1116  goal.planning_options.look_around = can_look_;
1117  goal.planning_options.replan = can_replan_;
1118  goal.planning_options.replan_delay = replan_delay_;
1119  goal.planning_options.planning_scene_diff.is_diff = true;
1120  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1121  return goal;
1122  }
1123 
1124  moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
1125  std::vector<moveit_msgs::PlaceLocation>&& locations,
1126  bool plan_only = false) const
1127  {
1128  moveit_msgs::PlaceGoal goal;
1129  goal.group_name = opt_.group_name_;
1130  goal.attached_object_name = object;
1131  goal.support_surface_name = support_surface_;
1132  goal.place_locations = std::move(locations);
1133  if (!support_surface_.empty())
1134  goal.allow_gripper_support_collision = true;
1135 
1136  if (path_constraints_)
1137  goal.path_constraints = *path_constraints_;
1138 
1139  goal.planner_id = planner_id_;
1140  goal.allowed_planning_time = allowed_planning_time_;
1141 
1142  goal.planning_options.plan_only = plan_only;
1143  goal.planning_options.look_around = can_look_;
1144  goal.planning_options.replan = can_replan_;
1145  goal.planning_options.replan_delay = replan_delay_;
1146  goal.planning_options.planning_scene_diff.is_diff = true;
1147  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1148  return goal;
1149  }
1150 
1151  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1152  {
1153  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1154  }
1155 
1156  bool setPathConstraints(const std::string& constraint)
1157  {
1159  {
1161  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1162  {
1163  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1164  return true;
1165  }
1166  else
1167  return false;
1168  }
1169  else
1170  return false;
1171  }
1172 
1173  void clearPathConstraints()
1174  {
1175  path_constraints_.reset();
1176  }
1177 
1178  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1179  {
1180  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1181  }
1182 
1184  {
1185  trajectory_constraints_.reset();
1186  }
1187 
1188  std::vector<std::string> getKnownConstraints() const
1189  {
1191  {
1192  static ros::WallDuration d(0.01);
1193  d.sleep();
1194  }
1195 
1196  std::vector<std::string> c;
1198  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1199 
1200  return c;
1201  }
1202 
1203  moveit_msgs::Constraints getPathConstraints() const
1204  {
1205  if (path_constraints_)
1206  return *path_constraints_;
1207  else
1208  return moveit_msgs::Constraints();
1209  }
1210 
1211  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1212  {
1214  return *trajectory_constraints_;
1215  else
1216  return moveit_msgs::TrajectoryConstraints();
1217  }
1218 
1219  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1220  {
1223  constraints_init_thread_->join();
1225  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1226  }
1227 
1228  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1229  {
1230  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1231  workspace_parameters_.header.stamp = ros::Time::now();
1232  workspace_parameters_.min_corner.x = minx;
1233  workspace_parameters_.min_corner.y = miny;
1234  workspace_parameters_.min_corner.z = minz;
1235  workspace_parameters_.max_corner.x = maxx;
1236  workspace_parameters_.max_corner.y = maxy;
1237  workspace_parameters_.max_corner.z = maxz;
1238  }
1239 
1240 private:
1241  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1242  {
1243  // Set up db
1244  try
1245  {
1247  conn->setParams(host, port);
1248  if (conn->connect())
1249  {
1251  }
1252  }
1253  catch (std::exception& ex)
1254  {
1255  ROS_ERROR_NAMED(LOGNAME, "%s", ex.what());
1256  }
1257  initializing_constraints_ = false;
1258  }
1259 
1260  Options opt_;
1262  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1263  moveit::core::RobotModelConstPtr robot_model_;
1264  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1265  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1266  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1267  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1268  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1269 
1270  // general planning params
1271  moveit::core::RobotStatePtr considered_start_state_;
1272  moveit_msgs::WorkspaceParameters workspace_parameters_;
1273  double allowed_planning_time_;
1274  std::string planner_id_;
1275  unsigned int num_planning_attempts_;
1278  double goal_joint_tolerance_;
1281  bool can_look_;
1282  bool can_replan_;
1283  double replan_delay_;
1284 
1285  // joint state goal
1286  moveit::core::RobotStatePtr joint_state_target_;
1288 
1289  // pose goal;
1290  // for each link we have a set of possible goal locations;
1291  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1292 
1293  // common properties for goals
1294  ActiveTargetType active_target_;
1295  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1296  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1297  std::string end_effector_link_;
1298  std::string pose_reference_frame_;
1299  std::string support_surface_;
1300 
1301  // ROS communication
1309  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1310  std::unique_ptr<boost::thread> constraints_init_thread_;
1312 };
1313 
1314 MoveGroupInterface::MoveGroupInterface(const std::string& group_name, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1315  const ros::WallDuration& wait_for_servers)
1316 {
1317  if (!ros::ok())
1318  throw std::runtime_error("ROS does not seem to be running");
1319  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1320 }
1321 
1322 MoveGroupInterface::MoveGroupInterface(const std::string& group, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1323  const ros::Duration& wait_for_servers)
1324  : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1325 {
1326 }
1327 
1328 MoveGroupInterface::MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1329  const ros::WallDuration& wait_for_servers)
1331  impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1335  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1336  const ros::Duration& wait_for_servers)
1337  : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1338 {
1343  delete impl_;
1347  : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1349  other.impl_ = nullptr;
1353 {
1354  if (this != &other)
1355  {
1356  delete impl_;
1357  impl_ = other.impl_;
1358  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1359  other.impl_ = nullptr;
1360  }
1361 
1362  return *this;
1365 const std::string& MoveGroupInterface::getName() const
1368 }
1369 
1370 const std::vector<std::string>& MoveGroupInterface::getNamedTargets() const
1372  // The pointer returned by getJointModelGroup is guaranteed by the class
1373  // constructor to always be non-null
1377 moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const
1380 }
1381 
1383 {
1384  return impl_->getOptions().node_handle_;
1385 }
1386 
1387 bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const
1388 {
1389  return impl_->getInterfaceDescription(desc);
1391 
1392 std::map<std::string, std::string> MoveGroupInterface::getPlannerParams(const std::string& planner_id,
1393  const std::string& group) const
1394 {
1395  return impl_->getPlannerParams(planner_id, group);
1397 
1398 void MoveGroupInterface::setPlannerParams(const std::string& planner_id, const std::string& group,
1399  const std::map<std::string, std::string>& params, bool replace)
1400 {
1401  impl_->setPlannerParams(planner_id, group, params, replace);
1403 
1404 std::string MoveGroupInterface::getDefaultPlannerId(const std::string& group) const
1405 {
1406  return impl_->getDefaultPlannerId(group);
1407 }
1408 
1409 void MoveGroupInterface::setPlannerId(const std::string& planner_id)
1410 {
1411  impl_->setPlannerId(planner_id);
1412 }
1413 
1414 const std::string& MoveGroupInterface::getPlannerId() const
1415 {
1416  return impl_->getPlannerId();
1417 }
1418 
1419 void MoveGroupInterface::setNumPlanningAttempts(unsigned int num_planning_attempts)
1421  impl_->setNumPlanningAttempts(num_planning_attempts);
1422 }
1423 
1424 void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
1425 {
1426  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1427 }
1428 
1429 void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
1430 {
1431  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1432 }
1435 {
1436  return impl_->move(false);
1437 }
1440 {
1441  return impl_->getMoveGroupClient();
1442 }
1443 
1446  return impl_->move(true);
1447 }
1448 
1451  return impl_->execute(plan.trajectory_, false);
1452 }
1453 
1454 MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
1456  return impl_->execute(trajectory, false);
1457 }
1458 
1461  return impl_->execute(plan.trajectory_, true);
1462 }
1463 
1464 MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
1465 {
1466  return impl_->execute(trajectory, true);
1467 }
1468 
1470 {
1471  return impl_->plan(plan);
1473 
1474 moveit_msgs::PickupGoal MoveGroupInterface::constructPickupGoal(const std::string& object,
1475  std::vector<moveit_msgs::Grasp> grasps,
1476  bool plan_only = false) const
1478  return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
1479 }
1480 
1481 moveit_msgs::PlaceGoal MoveGroupInterface::constructPlaceGoal(const std::string& object,
1482  std::vector<moveit_msgs::PlaceLocation> locations,
1483  bool plan_only = false) const
1484 {
1485  return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
1486 }
1488 std::vector<moveit_msgs::PlaceLocation>
1489 MoveGroupInterface::posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses) const
1490 {
1491  return impl_->posesToPlaceLocations(poses);
1493 
1494 MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
1495 {
1496  return impl_->pick(goal);
1498 
1499 MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
1500 {
1501  return impl_->planGraspsAndPick(object, plan_only);
1503 
1504 MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only)
1505 {
1506  return impl_->planGraspsAndPick(object, plan_only);
1508 
1509 MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
1510 {
1511  return impl_->place(goal);
1513 
1514 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step,
1515  double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1516  bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1518  moveit_msgs::Constraints path_constraints_tmp;
1519  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1520  error_code);
1521 }
1523 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step,
1524  double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1525  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1526  moveit_msgs::MoveItErrorCodes* error_code)
1528  if (error_code)
1529  {
1530  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1531  avoid_collisions, *error_code);
1532  }
1533  else
1534  {
1535  moveit_msgs::MoveItErrorCodes error_code_tmp;
1536  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1537  avoid_collisions, error_code_tmp);
1538  }
1539 }
1540 
1543  impl_->stop();
1544 }
1545 
1546 void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1547 {
1548  moveit::core::RobotStatePtr rs;
1550  moveit::core::robotStateMsgToRobotState(start_state, *rs);
1551  setStartState(*rs);
1552 }
1553 
1555 {
1556  impl_->setStartState(start_state);
1558 
1560 {
1563 
1565 {
1568 }
1569 
1570 const std::vector<std::string>& MoveGroupInterface::getJointNames() const
1571 {
1573 }
1574 
1575 const std::vector<std::string>& MoveGroupInterface::getLinkNames() const
1576 {
1578 }
1579 
1580 std::map<std::string, double> MoveGroupInterface::getNamedTargetValues(const std::string& name) const
1581 {
1582  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1583  std::map<std::string, double> positions;
1584 
1585  if (it != remembered_joint_values_.cend())
1586  {
1587  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1588  for (size_t x = 0; x < names.size(); ++x)
1589  {
1590  positions[names[x]] = it->second[x];
1591  }
1592  }
1593  else
1594  {
1596  }
1597  return positions;
1598 }
1599 
1600 bool MoveGroupInterface::setNamedTarget(const std::string& name)
1601 {
1602  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1603  if (it != remembered_joint_values_.end())
1604  {
1605  return setJointValueTarget(it->second);
1606  }
1607  else
1608  {
1610  {
1611  impl_->setTargetType(JOINT);
1612  return true;
1613  }
1614  ROS_ERROR_NAMED(LOGNAME, "The requested named target '%s' does not exist", name.c_str());
1615  return false;
1616  }
1617 }
1618 
1619 void MoveGroupInterface::getJointValueTarget(std::vector<double>& group_variable_values) const
1620 {
1623 
1624 bool MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1625 {
1626  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1627  return false;
1628  impl_->setTargetType(JOINT);
1631 }
1633 bool MoveGroupInterface::setJointValueTarget(const std::map<std::string, double>& variable_values)
1634 {
1635  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1636  for (const auto& pair : variable_values)
1637  {
1638  if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1639  {
1640  ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group "
1641  << impl_->getJointModelGroup()->getName());
1642  return false;
1643  }
1644  }
1645 
1646  impl_->setTargetType(JOINT);
1647  impl_->getTargetRobotState().setVariablePositions(variable_values);
1649 }
1650 
1651 bool MoveGroupInterface::setJointValueTarget(const std::vector<std::string>& variable_names,
1652  const std::vector<double>& variable_values)
1653 {
1654  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1655  for (const auto& variable_name : variable_names)
1656  {
1657  if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1658  {
1659  ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group "
1660  << impl_->getJointModelGroup()->getName());
1661  return false;
1662  }
1663  }
1664 
1665  impl_->setTargetType(JOINT);
1666  impl_->getTargetRobotState().setVariablePositions(variable_names, variable_values);
1669 
1671 {
1672  impl_->setTargetType(JOINT);
1673  impl_->getTargetRobotState() = rstate;
1675 }
1676 
1677 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1678 {
1679  std::vector<double> values(1, value);
1680  return setJointValueTarget(joint_name, values);
1681 }
1682 
1683 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, const std::vector<double>& values)
1684 {
1685  impl_->setTargetType(JOINT);
1686  const moveit::core::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name);
1687  if (jm && jm->getVariableCount() == values.size())
1688  {
1691  }
1693  ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName());
1694  return false;
1695 }
1696 
1697 bool MoveGroupInterface::setJointValueTarget(const sensor_msgs::JointState& state)
1698 {
1699  return setJointValueTarget(state.name, state.position);
1700 }
1702 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1703 {
1704  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1705 }
1706 
1707 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1708  const std::string& end_effector_link)
1709 {
1710  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1711 }
1712 
1713 bool MoveGroupInterface::setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link)
1714 {
1715  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1716  return setJointValueTarget(msg, end_effector_link);
1717 }
1718 
1719 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose,
1720  const std::string& end_effector_link)
1721 {
1722  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1723 }
1724 
1725 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1726  const std::string& end_effector_link)
1727 {
1728  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1729 }
1730 
1731 bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose,
1732  const std::string& end_effector_link)
1733 {
1734  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1735  return setApproximateJointValueTarget(msg, end_effector_link);
1736 }
1737 
1739 {
1740  return impl_->getTargetRobotState();
1741 }
1742 
1744 {
1746 }
1747 
1748 const std::string& MoveGroupInterface::getEndEffectorLink() const
1749 {
1750  return impl_->getEndEffectorLink();
1752 
1753 const std::string& MoveGroupInterface::getEndEffector() const
1754 {
1755  return impl_->getEndEffector();
1756 }
1757 
1758 bool MoveGroupInterface::setEndEffectorLink(const std::string& link_name)
1759 {
1760  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1761  return false;
1762  impl_->setEndEffectorLink(link_name);
1763  impl_->setTargetType(POSE);
1764  return true;
1766 
1767 bool MoveGroupInterface::setEndEffector(const std::string& eef_name)
1768 {
1769  const moveit::core::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1770  if (jmg)
1771  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1772  return false;
1773 }
1774 
1775 void MoveGroupInterface::clearPoseTarget(const std::string& end_effector_link)
1776 {
1777  impl_->clearPoseTarget(end_effector_link);
1778 }
1779 
1783 }
1784 
1785 bool MoveGroupInterface::setPoseTarget(const Eigen::Isometry3d& pose, const std::string& end_effector_link)
1786 {
1787  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1788  pose_msg[0].pose = tf2::toMsg(pose);
1789  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1790  pose_msg[0].header.stamp = ros::Time::now();
1791  return setPoseTargets(pose_msg, end_effector_link);
1792 }
1794 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link)
1795 {
1796  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1797  pose_msg[0].pose = target;
1798  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1799  pose_msg[0].header.stamp = ros::Time::now();
1800  return setPoseTargets(pose_msg, end_effector_link);
1801 }
1802 
1803 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link)
1804 {
1805  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1806  return setPoseTargets(targets, end_effector_link);
1807 }
1808 
1809 bool MoveGroupInterface::setPoseTargets(const EigenSTL::vector_Isometry3d& target, const std::string& end_effector_link)
1810 {
1811  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1812  ros::Time tm = ros::Time::now();
1813  const std::string& frame_id = getPoseReferenceFrame();
1814  for (std::size_t i = 0; i < target.size(); ++i)
1815  {
1816  pose_out[i].pose = tf2::toMsg(target[i]);
1817  pose_out[i].header.stamp = tm;
1818  pose_out[i].header.frame_id = frame_id;
1819  }
1820  return setPoseTargets(pose_out, end_effector_link);
1822 
1823 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1824  const std::string& end_effector_link)
1825 {
1826  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1827  ros::Time tm = ros::Time::now();
1828  const std::string& frame_id = getPoseReferenceFrame();
1829  for (std::size_t i = 0; i < target.size(); ++i)
1830  {
1831  target_stamped[i].pose = target[i];
1832  target_stamped[i].header.stamp = tm;
1833  target_stamped[i].header.frame_id = frame_id;
1834  }
1835  return setPoseTargets(target_stamped, end_effector_link);
1836 }
1837 
1838 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target,
1839  const std::string& end_effector_link)
1840 {
1841  if (target.empty())
1842  {
1843  ROS_ERROR_NAMED(LOGNAME, "No pose specified as goal target");
1844  return false;
1845  }
1846  else
1847  {
1849  return impl_->setPoseTargets(target, end_effector_link);
1850  }
1851 }
1852 
1853 const geometry_msgs::PoseStamped& MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1854 {
1855  return impl_->getPoseTarget(end_effector_link);
1856 }
1857 
1858 const std::vector<geometry_msgs::PoseStamped>&
1859 MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1860 {
1861  return impl_->getPoseTargets(end_effector_link);
1863 
1864 namespace
1865 {
1866 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1867  geometry_msgs::PoseStamped& target)
1868 {
1869  if (desired_frame != target.header.frame_id)
1870  {
1871  geometry_msgs::PoseStamped target_in(target);
1872  tf_buffer.transform(target_in, target, desired_frame);
1873  // we leave the stamp to ros::Time(0) on purpose
1874  target.header.stamp = ros::Time(0);
1875  }
1876 }
1877 } // namespace
1878 
1879 bool MoveGroupInterface::setPositionTarget(double x, double y, double z, const std::string& end_effector_link)
1880 {
1881  geometry_msgs::PoseStamped target;
1882  if (impl_->hasPoseTarget(end_effector_link))
1883  {
1884  target = getPoseTarget(end_effector_link);
1885  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1886  }
1887  else
1888  {
1889  target.pose.orientation.x = 0.0;
1890  target.pose.orientation.y = 0.0;
1891  target.pose.orientation.z = 0.0;
1892  target.pose.orientation.w = 1.0;
1893  target.header.frame_id = impl_->getPoseReferenceFrame();
1894  }
1895 
1896  target.pose.position.x = x;
1897  target.pose.position.y = y;
1898  target.pose.position.z = z;
1899  bool result = setPoseTarget(target, end_effector_link);
1900  impl_->setTargetType(POSITION);
1901  return result;
1902 }
1903 
1904 bool MoveGroupInterface::setRPYTarget(double r, double p, double y, const std::string& end_effector_link)
1905 {
1906  geometry_msgs::PoseStamped target;
1907  if (impl_->hasPoseTarget(end_effector_link))
1908  {
1909  target = getPoseTarget(end_effector_link);
1910  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1911  }
1912  else
1913  {
1914  target.pose.position.x = 0.0;
1915  target.pose.position.y = 0.0;
1916  target.pose.position.z = 0.0;
1917  target.header.frame_id = impl_->getPoseReferenceFrame();
1918  }
1920  q.setRPY(r, p, y);
1921  target.pose.orientation = tf2::toMsg(q);
1922  bool result = setPoseTarget(target, end_effector_link);
1923  impl_->setTargetType(ORIENTATION);
1924  return result;
1925 }
1926 
1927 bool MoveGroupInterface::setOrientationTarget(double x, double y, double z, double w,
1928  const std::string& end_effector_link)
1929 {
1930  geometry_msgs::PoseStamped target;
1931  if (impl_->hasPoseTarget(end_effector_link))
1932  {
1933  target = getPoseTarget(end_effector_link);
1934  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1935  }
1936  else
1937  {
1938  target.pose.position.x = 0.0;
1939  target.pose.position.y = 0.0;
1940  target.pose.position.z = 0.0;
1941  target.header.frame_id = impl_->getPoseReferenceFrame();
1942  }
1943 
1944  target.pose.orientation.x = x;
1945  target.pose.orientation.y = y;
1946  target.pose.orientation.z = z;
1947  target.pose.orientation.w = w;
1948  bool result = setPoseTarget(target, end_effector_link);
1949  impl_->setTargetType(ORIENTATION);
1950  return result;
1951 }
1952 
1953 void MoveGroupInterface::setPoseReferenceFrame(const std::string& pose_reference_frame)
1954 {
1955  impl_->setPoseReferenceFrame(pose_reference_frame);
1956 }
1957 
1958 const std::string& MoveGroupInterface::getPoseReferenceFrame() const
1959 {
1960  return impl_->getPoseReferenceFrame();
1961 }
1962 
1964 {
1965  return impl_->getGoalJointTolerance();
1966 }
1967 
1969 {
1970  return impl_->getGoalPositionTolerance();
1971 }
1974 {
1976 }
1977 
1978 void MoveGroupInterface::setGoalTolerance(double tolerance)
1979 {
1980  setGoalJointTolerance(tolerance);
1981  setGoalPositionTolerance(tolerance);
1982  setGoalOrientationTolerance(tolerance);
1983 }
1984 
1985 void MoveGroupInterface::setGoalJointTolerance(double tolerance)
1986 {
1987  impl_->setGoalJointTolerance(tolerance);
1988 }
1989 
1990 void MoveGroupInterface::setGoalPositionTolerance(double tolerance)
1991 {
1992  impl_->setGoalPositionTolerance(tolerance);
1993 }
1994 
1996 {
1998 }
1999 
2000 void MoveGroupInterface::rememberJointValues(const std::string& name)
2001 {
2003 }
2004 
2005 bool MoveGroupInterface::startStateMonitor(double wait)
2006 {
2007  return impl_->startStateMonitor(wait);
2008 }
2009 
2010 std::vector<double> MoveGroupInterface::getCurrentJointValues() const
2011 {
2012  moveit::core::RobotStatePtr current_state;
2013  std::vector<double> values;
2014  if (impl_->getCurrentState(current_state))
2015  current_state->copyJointGroupPositions(getName(), values);
2016  return values;
2017 }
2018 
2019 std::vector<double> MoveGroupInterface::getRandomJointValues() const
2020 {
2021  std::vector<double> r;
2023  return r;
2024 }
2025 
2026 geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) const
2027 {
2028  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2029  Eigen::Isometry3d pose;
2030  pose.setIdentity();
2031  if (eef.empty())
2032  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2033  else
2034  {
2035  moveit::core::RobotStatePtr current_state;
2036  if (impl_->getCurrentState(current_state))
2037  {
2038  current_state->setToRandomPositions(impl_->getJointModelGroup());
2039  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2040  if (lm)
2041  pose = current_state->getGlobalLinkTransform(lm);
2042  }
2043  }
2044  geometry_msgs::PoseStamped pose_msg;
2045  pose_msg.header.stamp = ros::Time::now();
2046  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2047  pose_msg.pose = tf2::toMsg(pose);
2048  return pose_msg;
2049 }
2050 
2051 geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) const
2052 {
2053  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2054  Eigen::Isometry3d pose;
2055  pose.setIdentity();
2056  if (eef.empty())
2057  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2058  else
2059  {
2060  moveit::core::RobotStatePtr current_state;
2061  if (impl_->getCurrentState(current_state))
2062  {
2063  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2064  if (lm)
2065  pose = current_state->getGlobalLinkTransform(lm);
2066  }
2067  }
2068  geometry_msgs::PoseStamped pose_msg;
2069  pose_msg.header.stamp = ros::Time::now();
2070  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2071  pose_msg.pose = tf2::toMsg(pose);
2072  return pose_msg;
2074 
2075 std::vector<double> MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) const
2076 {
2077  std::vector<double> result;
2078  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2079  if (eef.empty())
2080  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2081  else
2082  {
2083  moveit::core::RobotStatePtr current_state;
2084  if (impl_->getCurrentState(current_state))
2085  {
2086  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2087  if (lm)
2088  {
2089  result.resize(3);
2090  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2091  double pitch, roll, yaw;
2092  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2093  result[0] = roll;
2094  result[1] = pitch;
2095  result[2] = yaw;
2096  }
2097  }
2098  }
2099  return result;
2100 }
2101 
2102 const std::vector<std::string>& MoveGroupInterface::getActiveJoints() const
2103 {
2105 }
2106 
2107 const std::vector<std::string>& MoveGroupInterface::getJoints() const
2108 {
2110 }
2111 
2112 unsigned int MoveGroupInterface::getVariableCount() const
2113 {
2115 }
2116 
2117 moveit::core::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const
2118 {
2119  moveit::core::RobotStatePtr current_state;
2120  impl_->getCurrentState(current_state, wait);
2121  return current_state;
2122 }
2123 
2124 void MoveGroupInterface::rememberJointValues(const std::string& name, const std::vector<double>& values)
2125 {
2127 }
2128 
2129 void MoveGroupInterface::forgetJointValues(const std::string& name)
2130 {
2131  remembered_joint_values_.erase(name);
2132 }
2133 
2134 void MoveGroupInterface::allowLooking(bool flag)
2135 {
2136  impl_->allowLooking(flag);
2137 }
2138 
2140 {
2141  impl_->allowReplanning(flag);
2142 }
2144 std::vector<std::string> MoveGroupInterface::getKnownConstraints() const
2145 {
2146  return impl_->getKnownConstraints();
2147 }
2148 
2149 moveit_msgs::Constraints MoveGroupInterface::getPathConstraints() const
2150 {
2151  return impl_->getPathConstraints();
2152 }
2153 
2154 bool MoveGroupInterface::setPathConstraints(const std::string& constraint)
2155 {
2156  return impl_->setPathConstraints(constraint);
2157 }
2158 
2159 void MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2160 {
2161  impl_->setPathConstraints(constraint);
2162 }
2163 
2165 {
2167 }
2168 
2169 moveit_msgs::TrajectoryConstraints MoveGroupInterface::getTrajectoryConstraints() const
2171  return impl_->getTrajectoryConstraints();
2172 }
2173 
2174 void MoveGroupInterface::setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
2176  impl_->setTrajectoryConstraints(constraint);
2177 }
2178 
2182 }
2183 
2184 void MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2186  impl_->initializeConstraintsStorage(host, port);
2187 }
2188 
2189 void MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
2190 {
2191  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2193 
2195 void MoveGroupInterface::setPlanningTime(double seconds)
2196 {
2198 }
2199 
2203  return impl_->getPlanningTime();
2204 }
2205 
2206 void MoveGroupInterface::setSupportSurfaceName(const std::string& name)
2209 }
2210 
2211 const std::string& MoveGroupInterface::getPlanningFrame() const
2213  return impl_->getRobotModel()->getModelFrame();
2214 }
2215 
2216 const std::vector<std::string>& MoveGroupInterface::getJointModelGroupNames() const
2218  return impl_->getRobotModel()->getJointModelGroupNames();
2219 }
2220 
2221 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2223  return attachObject(object, link, std::vector<std::string>());
2224 }
2225 
2226 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2227  const std::vector<std::string>& touch_links)
2228 {
2229  return impl_->attachObject(object, link, touch_links);
2230 }
2231 
2232 bool MoveGroupInterface::detachObject(const std::string& name)
2233 {
2234  return impl_->detachObject(name);
2235 }
2236 
2237 void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& goal_out)
2238 {
2239  impl_->constructMotionPlanRequest(goal_out);
2240 }
2241 
2242 } // namespace planning_interface
2243 } // namespace moveit
moveit::core::JointModelGroup::hasLinkModel
bool hasLinkModel(const std::string &link) const
moveit::core::LinkModel
response
const std::string response
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:907
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setJointValueTarget
bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
Definition: move_group_interface.cpp:473
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:1821
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:1614
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:1460
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
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:1537
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:1853
ros::WallDuration::sleep
bool sleep() const
moveit::core::JointModelGroup::getJointModel
const JointModel * getJointModel(const std::string &joint) const
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:1692
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::max_acceleration_scaling_factor_
double max_acceleration_scaling_factor_
Definition: move_group_interface.cpp:1345
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
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:2212
moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
Definition: move_group_interface.cpp:2237
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlanningTime
void setPlanningTime(double seconds)
Definition: move_group_interface.cpp:1069
kinematics::KinematicsQueryOptions::return_approximate_solution
bool return_approximate_solution
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTargetRobotState
moveit::core::RobotState & getTargetRobotState()
Definition: move_group_interface.cpp:441
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:2175
ros::Publisher
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:1507
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:2073
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::hasPoseTarget
bool hasPoseTarget(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:578
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::computeCartesianPath
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double step, double jump_threshold, moveit_msgs::RobotTrajectory &msg, const moveit_msgs::Constraints &path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
Definition: move_group_interface.cpp:939
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:2063
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::waitForAction
void waitForAction(const T &action, const std::string &name, const ros::WallTime &timeout, double allotted_time) const
Definition: move_group_interface.cpp:251
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:1995
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl
Definition: move_group_interface.cpp:161
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::opt_
Options opt_
Definition: move_group_interface.cpp:1328
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::MoveGroupInterfaceImpl
MoveGroupInterfaceImpl(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const ros::WallDuration &wait_for_servers)
Definition: move_group_interface.cpp:164
boost::shared_ptr
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::startStateMonitor
bool startStateMonitor(double wait)
Definition: move_group_interface.cpp:640
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearTrajectoryConstraints
void clearTrajectoryConstraints()
Definition: move_group_interface.cpp:1251
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:1627
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::get_params_service_
ros::ServiceClient get_params_service_
Definition: move_group_interface.cpp:1373
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseTarget
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:584
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:2185
moveit::core::JointModelGroup::getActiveJointModelNames
const std::vector< std::string > & getActiveJointModelNames() const
moveit::planning_interface::MoveGroupInterface
Client class to conveniently use the ROS interfaces provided by the move_group node.
Definition: move_group_interface.h:165
ros::ServiceClient::exists
bool exists()
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTF
const std::shared_ptr< tf2_ros::Buffer > & getTF() const
Definition: move_group_interface.cpp:312
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::planGraspsAndPick
MoveItErrorCode planGraspsAndPick(const std::string &object, bool plan_only=false)
Definition: move_group_interface.cpp:765
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalOrientationTolerance
double getGoalOrientationTolerance() const
Definition: move_group_interface.cpp:1044
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
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:2207
utils.h
s
XmlRpcServer s
ros
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalJointTolerance
double getGoalJointTolerance() const
Definition: move_group_interface.cpp:1049
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
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:2180
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setReplanningDelay
void setReplanningDelay(double delay)
Definition: move_group_interface.cpp:1092
ros.h
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::GRASP_PLANNING_SERVICE_NAME
const std::string GRASP_PLANNING_SERVICE_NAME
Definition: move_group_interface.cpp:146
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::workspace_parameters_
moveit_msgs::WorkspaceParameters workspace_parameters_
Definition: move_group_interface.cpp:1340
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:1512
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPathConstraints
moveit_msgs::Constraints getPathConstraints() const
Definition: move_group_interface.cpp:1271
moveit::planning_interface::LOGNAME
const std::string LOGNAME
Definition: move_group_interface.cpp:148
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseTargets
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:600
moveit::planning_interface::MoveGroupInterface::asyncExecute
MoveItErrorCode asyncExecute(const Plan &plan)
Given a plan, execute it without waiting for completion.
Definition: move_group_interface.cpp:1517
constraints_storage.h
moveit::planning_interface::MoveGroupInterface::forgetJointValues
void forgetJointValues(const std::string &name)
Forget the joint values remembered under name.
Definition: move_group_interface.cpp:2197
r
r
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:2279
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPathConstraints
void clearPathConstraints()
Definition: move_group_interface.cpp:1241
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: move_group_interface.cpp:1331
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::stop
void stop()
Definition: move_group_interface.cpp:979
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::plan_grasps_service_
ros::ServiceClient plan_grasps_service_
Definition: move_group_interface.cpp:1376
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
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:1477
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:2053
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:2143
moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
Definition: move_group_interface.cpp:2284
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::joint_model_group_
const moveit::core::JointModelGroup * joint_model_group_
Definition: move_group_interface.cpp:1355
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getOptions
const Options & getOptions() const
Definition: move_group_interface.cpp:317
moveit::planning_interface::PlanningSceneInterface
Definition: planning_scene_interface.h:116
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::can_look_
bool can_look_
Definition: move_group_interface.cpp:1349
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPathConstraints
void setPathConstraints(const moveit_msgs::Constraints &constraint)
Definition: move_group_interface.cpp:1219
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::move
MoveItErrorCode move(bool wait)
Definition: move_group_interface.cpp:855
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::cartesian_path_service_
ros::ServiceClient cartesian_path_service_
Definition: move_group_interface.cpp:1375
ROS_WARN_ONCE_NAMED
#define ROS_WARN_ONCE_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalPositionTolerance
double getGoalPositionTolerance() const
Definition: move_group_interface.cpp:1039
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
moveit::core::RobotState::copyJointGroupPositions
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
moveit::planning_interface::MoveGroupInterface::setRandomTarget
void setRandomTarget()
Set the joint state goal to a random joint configuration.
Definition: move_group_interface.cpp:1632
moveit::core::RobotState::setFromIK
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
step
unsigned int step
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:2078
moveit::planning_interface::MoveGroupInterface::setPlanningTime
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
Definition: move_group_interface.cpp:2263
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:2232
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constraints_init_thread_
std::unique_ptr< boost::thread > constraints_init_thread_
Definition: move_group_interface.cpp:1378
moveit_warehouse::ConstraintsStorage
moveit::planning_interface::MoveGroupInterface::planGraspsAndPick
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
Definition: move_group_interface.cpp:1567
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:1816
moveit::planning_interface::MoveGroupInterface::clearPoseTargets
void clearPoseTargets()
Forget any poses specified for all end-effectors.
Definition: move_group_interface.cpp:1848
moveit::planning_interface::MoveGroupInterface::getPoseTargets
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1927
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setEndEffectorLink
void setEndEffectorLink(const std::string &end_effector)
Definition: move_group_interface.cpp:525
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getEndEffector
const std::string & getEndEffector() const
Definition: move_group_interface.cpp:545
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
moveit::planning_interface::MoveGroupInterface::getTargetRobotState
const moveit::core::RobotState & getTargetRobotState() const
Definition: move_group_interface.cpp:1811
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:2068
ros::ok
ROSCPP_DECL bool ok()
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:1466
moveit::planning_interface::MoveGroupInterface::Options
Specification of options to use when constructing the MoveGroupInterface class.
Definition: move_group_interface.h:172
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:2058
moveit::planning_interface::MoveGroupInterface::remembered_joint_values_
std::map< std::string, std::vector< double > > remembered_joint_values_
Definition: move_group_interface.h:1072
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::trajectory_event_publisher_
ros::Publisher trajectory_event_publisher_
Definition: move_group_interface.cpp:1370
tf2::eigenToTransform
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxScalingFactor
void setMaxScalingFactor(double &variable, const double target_value, const char *factor_name, double fallback_value)
Definition: move_group_interface.cpp:419
moveit::planning_interface::getSharedTF
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
Definition: common_objects.cpp:99
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pick_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
Definition: move_group_interface.cpp:1335
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::~MoveGroupInterfaceImpl
~MoveGroupInterfaceImpl()
Definition: move_group_interface.cpp:306
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalPositionTolerance
void setGoalPositionTolerance(double tolerance)
Definition: move_group_interface.cpp:1059
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:1582
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:2202
moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner for a given group (or global default)
Definition: move_group_interface.cpp:1472
move_group::QUERY_PLANNERS_SERVICE_NAME
static const std::string QUERY_PLANNERS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTrajectoryConstraints
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
Definition: move_group_interface.cpp:1279
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute
MoveItErrorCode execute(const moveit_msgs::RobotTrajectory &trajectory, bool wait)
Definition: move_group_interface.cpp:900
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getReplanningDelay
double getReplanningDelay() const
Definition: move_group_interface.cpp:1098
moveit::core::JointModelGroup::getVariableNames
const std::vector< std::string > & getVariableNames() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setTrajectoryConstraints
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
Definition: move_group_interface.cpp:1246
moveit::core::RobotState::getRandomNumberGenerator
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
actionlib::SimpleActionClient
moveit::core::JointModelGroup::getName
const std::string & getName() const
move_group::MOVE_ACTION
static const std::string MOVE_ACTION
console.h
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:2026
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:2252
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pose_targets_
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
Definition: move_group_interface.cpp:1359
moveit::core::JointModelGroup::getVariableDefaultPositions
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
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:2021
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getDefaultPlannerId
std::string getDefaultPlannerId(const std::string &group) const
Definition: move_group_interface.cpp:381
moveit_warehouse::loadDatabase
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
res
res
move_group::SET_PLANNER_PARAMS_SERVICE_NAME
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::getRobotModel
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
Definition: move_group_interface.cpp:1445
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:2094
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPoseTargets
void clearPoseTargets()
Definition: move_group_interface.cpp:535
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::end_effector_link_
std::string end_effector_link_
Definition: move_group_interface.cpp:1365
moveit::planning_interface::MoveGroupInterface::execute
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion.
Definition: move_group_interface.cpp:1527
ros::CallbackQueue
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:1450
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::considered_start_state_
moveit::core::RobotStatePtr considered_start_state_
Definition: move_group_interface.cpp:1339
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface
~MoveGroupInterface()
Definition: move_group_interface.cpp:1409
capability_names.h
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getCurrentState
bool getCurrentState(moveit::core::RobotStatePtr &current_state, double wait_seconds=1.0)
Definition: move_group_interface.cpp:656
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructGoal
void constructGoal(moveit_msgs::MoveGroupGoal &goal) const
Definition: move_group_interface.cpp:1160
planning_scene_monitor.h
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:1492
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::num_planning_attempts_
unsigned int num_planning_attempts_
Definition: move_group_interface.cpp:1343
y
double y
ros::WallTime::now
static WallTime now()
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
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:1433
moveit::core::JointModelGroup::getJointModelNames
const std::vector< std::string > & getJointModelNames() const
trajectory_execution_manager.h
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:1497
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getKnownConstraints
std::vector< std::string > getKnownConstraints() const
Definition: move_group_interface.cpp:1256
moveit::planning_interface::MoveGroupInterface::Options::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_interface.h:189
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
kinematic_constraints::mergeConstraints
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
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:2046
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getJointModelGroup
const moveit::core::JointModelGroup * getJointModelGroup() const
Definition: move_group_interface.cpp:327
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:1787
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:1826
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxAccelerationScalingFactor
void setMaxAccelerationScalingFactor(double value)
Definition: move_group_interface.cpp:414
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setSupportSurfaceName
void setSupportSurfaceName(const std::string &support_surface)
Definition: move_group_interface.cpp:620
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: move_group_interface.cpp:1330
ros::ServiceClient
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:1972
setup.d
d
Definition: setup.py:4
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:1557
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::move_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > > move_action_client_
Definition: move_group_interface.cpp:1333
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::path_constraints_
std::unique_ptr< moveit_msgs::Constraints > path_constraints_
Definition: move_group_interface.cpp:1363
ros::NodeHandle::getCallbackQueue
CallbackQueueInterface * getCallbackQueue() const
tf2_ros::Buffer
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pose_reference_frame_
std::string pose_reference_frame_
Definition: move_group_interface.cpp:1366
moveit::planning_interface::MoveItErrorCode
Definition: move_group_interface.h:129
actionlib::SimpleClientGoalState::SUCCEEDED
SUCCEEDED
moveit::core::JointModelGroup::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::trajectory_constraints_
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
Definition: move_group_interface.cpp:1364
moveit::core::JointModelGroup::getDefaultStateNames
const std::vector< std::string > & getDefaultStateNames() const
moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
Definition: move_group_interface.cpp:2242
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowLooking
void allowLooking(bool flag)
Definition: move_group_interface.cpp:1080
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:2222
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::place
MoveItErrorCode place(const moveit_msgs::PlaceGoal &goal)
Definition: move_group_interface.cpp:704
moveit::planning_interface::MoveGroupInterface::stop
void stop()
Stop any trajectory execution, if one is active.
Definition: move_group_interface.cpp:1609
trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC
static const std::string EXECUTION_EVENT_TOPIC
ros::WallTime
q
q
moveit::planning_interface::MoveGroupInterface::getPoseTarget
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1921
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlannerParams
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool replace=false)
Definition: move_group_interface.cpp:365
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::active_target_
ActiveTargetType active_target_
Definition: move_group_interface.cpp:1362
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getStartState
moveit::core::RobotStatePtr getStartState()
Definition: move_group_interface.cpp:461
planning_scene_interface.h
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:2031
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::joint_state_target_
moveit::core::RobotStatePtr joint_state_target_
Definition: move_group_interface.cpp:1354
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setStartStateToCurrentState
void setStartStateToCurrentState()
Definition: move_group_interface.cpp:456
common_objects.h
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializeConstraintsStorage
void initializeConstraintsStorage(const std::string &host, unsigned int port)
Definition: move_group_interface.cpp:1287
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setTargetType
void setTargetType(ActiveTargetType type)
Definition: move_group_interface.cpp:630
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalJointTolerance
void setGoalJointTolerance(double tolerance)
Definition: move_group_interface.cpp:1054
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
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:2087
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializing_constraints_
bool initializing_constraints_
Definition: move_group_interface.cpp:1379
transform_listener.h
moveit::planning_interface::MoveGroupInterface::getPlannerId
const std::string & getPlannerId() const
Get the current planner_id.
Definition: move_group_interface.cpp:1482
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constraints_storage_
std::unique_ptr< moveit_warehouse::ConstraintsStorage > constraints_storage_
Definition: move_group_interface.cpp:1377
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:1806
moveit::planning_interface::getSharedStateMonitor
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const moveit::core::RobotModelCon...
Definition: common_objects.cpp:134
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlannerId
const std::string & getPlannerId() const
Definition: move_group_interface.cpp:399
ros::NodeHandle::ok
bool ok() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTargetType
ActiveTargetType getTargetType() const
Definition: move_group_interface.cpp:635
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructMotionPlanRequest
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request) const
Definition: move_group_interface.cpp:1103
moveit::planning_interface::MoveGroupInterface::operator=
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints
void clearTrajectoryConstraints()
Definition: move_group_interface.cpp:2247
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxVelocityScalingFactor
void setMaxVelocityScalingFactor(double value)
Definition: move_group_interface.cpp:409
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:1549
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
ros::Time
moveit
values
std::vector< double > values
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::can_replan_
bool can_replan_
Definition: move_group_interface.cpp:1350
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:1438
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:2274
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_joint_tolerance_
double goal_joint_tolerance_
Definition: move_group_interface.cpp:1346
current_state_monitor.h
kinematics::KinematicsQueryOptions
moveit::planning_interface::PlanningSceneInterface::getObjects
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
Definition: planning_scene_interface.cpp:376
tf_buffer
tf2_ros::Buffer * tf_buffer
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:1843
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:1487
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructPlaceGoal
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > &&locations, bool plan_only=false) const
Definition: move_group_interface.cpp:1192
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_orientation_tolerance_
double goal_orientation_tolerance_
Definition: move_group_interface.cpp:1348
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlannerParams
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
Definition: move_group_interface.cpp:350
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::query_service_
ros::ServiceClient query_service_
Definition: move_group_interface.cpp:1372
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::detachObject
bool detachObject(const std::string &name)
Definition: move_group_interface.cpp:1015
move_group_interface.h
moveit::planning_interface::getSharedRobotModel
moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
Definition: common_objects.cpp:116
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::replan_delay_
double replan_delay_
Definition: move_group_interface.cpp:1351
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlannerId
void setPlannerId(const std::string &planner_id)
Definition: move_group_interface.cpp:394
planning_interface
move_group::EXECUTE_ACTION_NAME
static const std::string EXECUTE_ACTION_NAME
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:1947
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructPickupGoal
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > &&grasps, bool plan_only=false) const
Definition: move_group_interface.cpp:1165
approx
bool approx(S x, S y)
moveit::planning_interface::MoveGroupInterface::getInterfaceDescription
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc) const
Get the description of the planning plugin loaded by the action server.
Definition: move_group_interface.cpp:1455
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::plan
MoveItErrorCode plan(Plan &plan)
Definition: move_group_interface.cpp:814
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setStartState
void setStartState(const moveit::core::RobotState &start_state)
Definition: move_group_interface.cpp:451
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: move_group_interface.cpp:322
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getInterfaceDescription
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
Definition: move_group_interface.cpp:337
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPoseTargets
bool setPoseTargets(const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &end_effector_link)
Definition: move_group_interface.cpp:559
moveit::core::JointModelGroup::isChain
bool isChain() const
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
moveit::core::JointModelGroup
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
tf2::Quaternion
tolerance
S tolerance()
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
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:2289
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:2257
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::attached_object_publisher_
ros::Publisher attached_object_publisher_
Definition: move_group_interface.cpp:1371
move_group::PICKUP_ACTION
static const std::string PICKUP_ACTION
moveit::planning_interface::MoveGroupInterface::pick
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
Definition: move_group_interface.h:873
tf2::toMsg
B toMsg(const A &a)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::current_state_monitor_
planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_
Definition: move_group_interface.cpp:1332
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getEndEffectorLink
const std::string & getEndEffectorLink() const
Definition: move_group_interface.cpp:540
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowReplanning
void allowReplanning(bool flag)
Definition: move_group_interface.cpp:1086
moveit::planning_interface::MoveGroupInterface::impl_
MoveGroupInterfaceImpl * impl_
Definition: move_group_interface.h:1073
param
T param(const std::string &param_name, const T &default_val)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_position_tolerance_
double goal_position_tolerance_
Definition: move_group_interface.cpp:1347
conversions.h
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:2217
x
double x
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalOrientationTolerance
void setGoalOrientationTolerance(double tolerance)
Definition: move_group_interface.cpp:1064
moveit::planning_interface::MoveGroupInterface::getPlanningTime
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
Definition: move_group_interface.cpp:2269
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:1396
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
Definition: move_group_interface.cpp:1334
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:2170
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:1643
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:1638
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::place_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
Definition: move_group_interface.cpp:1336
DurationBase< WallDuration >::toSec
double toSec() const
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:1877
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:1668
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setWorkspace
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Definition: move_group_interface.cpp:1296
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
move_group::CARTESIAN_PATH_SERVICE_NAME
static const std::string CARTESIAN_PATH_SERVICE_NAME
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:2041
ros::WallDuration
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::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:680
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::max_velocity_scaling_factor_
double max_velocity_scaling_factor_
Definition: move_group_interface.cpp:1344
seconds
FCL_EXPORT double seconds(const duration &d)
moveit::core::JointModelGroup::getEndEffectorParentGroup
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
move_group::GET_PLANNER_PARAMS_SERVICE_NAME
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializeConstraintsStorageThread
void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
Definition: move_group_interface.cpp:1309
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPoseTarget
void clearPoseTarget(const std::string &end_effector_link)
Definition: move_group_interface.cpp:530
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:1648
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:1502
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlanningTime
double getPlanningTime() const
Definition: move_group_interface.cpp:1075
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:2305
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pick
MoveItErrorCode pick(const moveit_msgs::PickupGoal &goal)
Definition: move_group_interface.cpp:735
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::attachObject
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
Definition: move_group_interface.cpp:989
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:2119
z
double z
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getMoveGroupClient
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Definition: move_group_interface.cpp:332
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:2300
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::support_surface_
std::string support_surface_
Definition: move_group_interface.cpp:1367
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:1835
ros::CallbackQueue::callAvailable
void callAvailable()
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
t
geometry_msgs::TransformStamped t
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowed_planning_time_
double allowed_planning_time_
Definition: move_group_interface.cpp:1341
moveit::core::JointModel
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setNumPlanningAttempts
void setNumPlanningAttempts(unsigned int num_planning_attempts)
Definition: move_group_interface.cpp:404
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::set_params_service_
ros::ServiceClient set_params_service_
Definition: move_group_interface.cpp:1374
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::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
name
name
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:1542
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPoseReferenceFrame
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Definition: move_group_interface.cpp:615
move_group::PLACE_ACTION
static const std::string PLACE_ACTION
ros::Time::now
static Time now()
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
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:2036
test_cleanup.group
group
Definition: test_cleanup.py:5
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_interface.cpp:1329
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::planner_id_
std::string planner_id_
Definition: move_group_interface.cpp:1342
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseReferenceFrame
const std::string & getPoseReferenceFrame() const
Definition: move_group_interface.cpp:625


planning_interface
Author(s): Ioan Sucan
autogenerated on Sat Dec 12 2020 03:27:08