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>
63 #include <std_msgs/String.h>
64 #include <geometry_msgs/TransformStamped.h>
80 const std::string
LOGNAME =
"move_group_interface";
93 class MoveGroupInterface::MoveGroupInterfaceImpl
105 std::string error =
"Unable to construct robot model. Please make sure all needed information is on the "
108 throw std::runtime_error(error);
113 std::string error =
"Group '" + opt.group_name_ +
"' was not found.";
115 throw std::runtime_error(error);
133 std::string desc = opt.robot_description_.length() ? opt.robot_description_ :
ROBOT_DESCRIPTION;
135 std::string kinematics_desc = desc +
"_kinematics/";
140 node_handle_.
param<
double>(kinematics_desc + opt.group_name_ +
"/goal_orientation_tolerance",
143 std::string planning_desc = desc +
"_planning/";
163 double allotted_time = wait_for_servers.
toSec();
165 move_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>>(
169 pick_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::PickupAction>>(
173 place_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::PlaceAction>>(
177 execute_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>>(
196 template <
typename T>
239 if (!action->isServerConnected())
241 std::stringstream error;
242 error <<
"Unable to connect to move_group action server '" <<
name <<
"' within allotted time (" << allotted_time
244 throw std::runtime_error(error.str());
258 const std::shared_ptr<tf2_ros::Buffer>&
getTF()
const
268 const moveit::core::RobotModelConstPtr&
getRobotModel()
const
285 moveit_msgs::QueryPlannerInterfaces::Request req;
286 moveit_msgs::QueryPlannerInterfaces::Response
res;
288 if (!
res.planner_interfaces.empty())
290 desc =
res.planner_interfaces.front();
298 moveit_msgs::QueryPlannerInterfaces::Request req;
299 moveit_msgs::QueryPlannerInterfaces::Response
res;
301 if (!
res.planner_interfaces.empty())
303 desc =
res.planner_interfaces;
309 std::map<std::string, std::string>
getPlannerParams(
const std::string& planner_id,
const std::string&
group =
"")
311 moveit_msgs::GetPlannerParams::Request req;
312 moveit_msgs::GetPlannerParams::Response
res;
313 req.planner_config = planner_id;
315 std::map<std::string, std::string> result;
318 for (
unsigned int i = 0, end =
res.params.keys.size(); i < end; ++i)
319 result[
res.params.keys[i]] =
res.params.values[i];
325 const std::map<std::string, std::string>& params,
bool replace =
false)
327 moveit_msgs::SetPlannerParams::Request req;
328 moveit_msgs::SetPlannerParams::Response
res;
329 req.planner_config = planner_id;
331 req.replace = replace;
332 for (
const std::pair<const std::string, std::string>& param : params)
334 req.params.keys.push_back(
param.first);
335 req.params.values.push_back(
param.second);
342 std::string default_planning_pipeline;
344 return default_planning_pipeline;
370 std::stringstream param_name;
371 param_name <<
"move_group";
372 if (!pipeline_id.empty())
373 param_name <<
"/planning_pipelines/" << pipeline_id;
375 param_name <<
"/" <<
group;
376 param_name <<
"/default_planner_config";
378 std::string default_planner_config;
380 return default_planner_config;
408 void setMaxScalingFactor(
double& variable,
const double target_value,
const char* factor_name,
double fallback_value)
410 if (target_value > 1.0)
415 else if (target_value <= 0.0)
417 node_handle_.
param<
double>(std::string(
"robot_description_planning/default_") + factor_name, variable,
419 if (target_value < 0.0)
426 variable = target_value;
452 void setStartState(
const moveit_msgs::RobotState& start_state)
472 moveit::core::RobotStatePtr
s;
478 bool setJointValueTarget(
const geometry_msgs::Pose& eef_pose,
const std::string& end_effector_link,
479 const std::string& frame,
bool approx)
481 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
554 const std::vector<std::string>& possible_eefs =
556 for (
const std::string& possible_eef : possible_eefs)
560 static std::string empty;
564 bool setPoseTargets(
const std::vector<geometry_msgs::PoseStamped>& poses,
const std::string& end_effector_link)
566 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
576 std::vector<geometry_msgs::PoseStamped>& stored_poses =
pose_targets_[eef];
577 for (geometry_msgs::PoseStamped& stored_pose : stored_poses)
583 bool hasPoseTarget(
const std::string& end_effector_link)
const
585 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
589 const geometry_msgs::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const
591 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
594 std::map<std::string, std::vector<geometry_msgs::PoseStamped>>::const_iterator jt =
pose_targets_.find(eef);
596 if (!jt->second.empty())
597 return jt->second.at(0);
600 static const geometry_msgs::PoseStamped UNKNOWN;
605 const std::vector<geometry_msgs::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const
607 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
609 std::map<std::string, std::vector<geometry_msgs::PoseStamped>>::const_iterator jt =
pose_targets_.find(eef);
611 if (!jt->second.empty())
615 static const std::vector<geometry_msgs::PoseStamped> EMPTY;
661 bool getCurrentState(moveit::core::RobotStatePtr& current_state,
double wait_seconds = 1.0)
684 std::vector<moveit_msgs::PlaceLocation>
687 std::vector<moveit_msgs::PlaceLocation> locations;
688 for (
const geometry_msgs::PoseStamped& pose : poses)
690 moveit_msgs::PlaceLocation location;
691 location.pre_place_approach.direction.vector.z = -1.0;
692 location.post_place_retreat.direction.vector.x = -1.0;
693 location.pre_place_approach.direction.header.frame_id =
getRobotModel()->getModelFrame();
696 location.pre_place_approach.min_distance = 0.1;
697 location.pre_place_approach.desired_distance = 0.2;
698 location.post_place_retreat.min_distance = 0.0;
699 location.post_place_retreat.desired_distance = 0.2;
702 location.place_pose = pose;
703 locations.push_back(location);
714 return moveit::core::MoveItErrorCode::FAILURE;
719 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
745 return moveit::core::MoveItErrorCode::FAILURE;
750 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
778 std::map<std::string, moveit_msgs::CollisionObject> objects = psi.
getObjects(std::vector<std::string>(1,
object));
783 "Asked for grasps for the object '" <<
object <<
"', but the object could not be found");
784 return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME;
796 <<
"' is not available."
797 " This has to be implemented and started separately.");
798 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
801 moveit_msgs::GraspPlanning::Request request;
802 moveit_msgs::GraspPlanning::Response
response;
805 request.target = object;
810 response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
813 return moveit::core::MoveItErrorCode::FAILURE;
824 return moveit::core::MoveItErrorCode::FAILURE;
829 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
832 moveit_msgs::MoveGroupGoal goal;
834 goal.planning_options.plan_only =
true;
835 goal.planning_options.look_around =
false;
836 goal.planning_options.replan =
false;
837 goal.planning_options.planning_scene_diff.is_diff =
true;
838 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
865 return moveit::core::MoveItErrorCode::FAILURE;
870 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
873 moveit_msgs::MoveGroupGoal goal;
875 goal.planning_options.plan_only =
false;
876 goal.planning_options.look_around =
can_look_;
879 goal.planning_options.planning_scene_diff.is_diff =
true;
880 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
885 return moveit::core::MoveItErrorCode::SUCCESS;
910 return moveit::core::MoveItErrorCode::FAILURE;
915 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
918 moveit_msgs::ExecuteTrajectoryGoal goal;
919 goal.trajectory = trajectory;
924 return moveit::core::MoveItErrorCode::SUCCESS;
944 double computeCartesianPath(
const std::vector<geometry_msgs::Pose>& waypoints,
double step,
double jump_threshold,
945 moveit_msgs::RobotTrajectory& msg,
const moveit_msgs::Constraints& path_constraints,
946 bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
948 moveit_msgs::GetCartesianPath::Request req;
949 moveit_msgs::GetCartesianPath::Response
res;
955 req.waypoints = waypoints;
957 req.jump_threshold = jump_threshold;
958 req.path_constraints = path_constraints;
959 req.avoid_collisions = avoid_collisions;
966 error_code =
res.error_code;
967 if (
res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
977 error_code.val = error_code.FAILURE;
986 std_msgs::String event;
992 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
1006 moveit_msgs::AttachedCollisionObject aco;
1007 aco.object.id = object;
1008 aco.link_name.swap(l);
1009 if (touch_links.empty())
1010 aco.touch_links.push_back(aco.link_name);
1012 aco.touch_links = touch_links;
1013 aco.object.operation = moveit_msgs::CollisionObject::ADD;
1020 moveit_msgs::AttachedCollisionObject aco;
1023 aco.link_name =
name;
1025 aco.object.id =
name;
1026 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1027 if (aco.link_name.empty() && aco.object.id.empty())
1031 for (
const std::string& lname : lnames)
1033 aco.link_name = lname;
1099 request.goal_constraints.resize(1);
1106 std::size_t goal_count = 0;
1108 goal_count = std::max(goal_count, pose_target.second.size());
1114 request.goal_constraints.resize(goal_count);
1118 for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1123 c.position_constraints.clear();
1125 c.orientation_constraints.clear();
1144 moveit_msgs::PickupGoal
constructPickupGoal(
const std::string&
object, std::vector<moveit_msgs::Grasp>&& grasps,
1145 bool plan_only =
false)
const
1147 moveit_msgs::PickupGoal goal;
1148 goal.target_name = object;
1152 goal.possible_grasps = std::move(grasps);
1154 goal.allow_gripper_support_collision =
true;
1162 goal.planning_options.plan_only = plan_only;
1163 goal.planning_options.look_around =
can_look_;
1166 goal.planning_options.planning_scene_diff.is_diff =
true;
1167 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1172 std::vector<moveit_msgs::PlaceLocation>&& locations,
1173 bool plan_only =
false)
const
1175 moveit_msgs::PlaceGoal goal;
1177 goal.attached_object_name = object;
1179 goal.place_locations = std::move(locations);
1181 goal.allow_gripper_support_collision =
true;
1189 goal.planning_options.plan_only = plan_only;
1190 goal.planning_options.look_around =
can_look_;
1193 goal.planning_options.planning_scene_diff.is_diff =
true;
1194 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1210 path_constraints_ = std::make_unique<moveit_msgs::Constraints>(
static_cast<moveit_msgs::Constraints
>(*msg_m));
1243 std::vector<std::string>
c;
1255 return moveit_msgs::Constraints();
1263 return moveit_msgs::TrajectoryConstraints();
1275 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
1294 conn->setParams(host, port);
1295 if (conn->connect())
1300 catch (std::exception& ex)
1312 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>>
move_action_client_;
1313 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>>
execute_action_client_;
1314 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction>>
pick_action_client_;
1370 throw std::runtime_error(
"ROS does not seem to be running");
1387 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1399 : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1401 other.impl_ =
nullptr;
1409 impl_ = other.
impl_;
1410 remembered_joint_values_ = std::move(other.remembered_joint_values_);
1411 other.impl_ =
nullptr;
1450 const std::string&
group)
const
1456 const std::map<std::string, std::string>& params,
bool replace)
1557 std::vector<moveit_msgs::Grasp> grasps,
1558 bool plan_only =
false)
const
1564 std::vector<moveit_msgs::PlaceLocation> locations,
1565 bool plan_only =
false)
const
1570 std::vector<moveit_msgs::PlaceLocation>
1598 double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1599 bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1601 return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::Constraints(),
1602 avoid_collisions, error_code);
1606 double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1607 const moveit_msgs::Constraints& path_constraints,
bool avoid_collisions,
1608 moveit_msgs::MoveItErrorCodes* error_code)
1610 moveit_msgs::MoveItErrorCodes err_tmp;
1611 moveit_msgs::MoveItErrorCodes& err = error_code ? *error_code : err_tmp;
1613 avoid_collisions, err);
1655 std::map<std::string, double> positions;
1660 for (
size_t x = 0;
x < names.size(); ++
x)
1662 positions[names[
x]] = it->second[
x];
1699 if (joint_values.size() != n_group_joints)
1703 <<
" has " << n_group_joints <<
" joints");
1714 for (
const auto& pair : variable_values)
1716 if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1718 ROS_ERROR_STREAM(
"joint variable " << pair.first <<
" is not part of group "
1730 const std::vector<double>& variable_values)
1732 if (variable_names.size() != variable_values.size())
1738 for (
const auto& variable_name : variable_names)
1740 if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1742 ROS_ERROR_STREAM(
"joint variable " << variable_name <<
" is not part of group "
1791 const std::string& end_effector_link)
1798 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1803 const std::string& end_effector_link)
1809 const std::string& end_effector_link)
1815 const std::string& end_effector_link)
1817 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1870 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1879 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1880 pose_msg[0].pose = target;
1888 std::vector<geometry_msgs::PoseStamped> targets(1, target);
1894 std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1897 for (std::size_t i = 0; i < target.size(); ++i)
1900 pose_out[i].header.stamp = tm;
1901 pose_out[i].header.frame_id = frame_id;
1907 const std::string& end_effector_link)
1909 std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1912 for (std::size_t i = 0; i < target.size(); ++i)
1914 target_stamped[i].pose = target[i];
1915 target_stamped[i].header.stamp = tm;
1916 target_stamped[i].header.frame_id = frame_id;
1922 const std::string& end_effector_link)
1941 const std::vector<geometry_msgs::PoseStamped>&
1950 geometry_msgs::PoseStamped& target)
1952 if (desired_frame != target.header.frame_id)
1954 geometry_msgs::PoseStamped target_in(target);
1964 geometry_msgs::PoseStamped target;
1972 target.pose.orientation.x = 0.0;
1973 target.pose.orientation.y = 0.0;
1974 target.pose.orientation.z = 0.0;
1975 target.pose.orientation.w = 1.0;
1979 target.pose.position.x =
x;
1980 target.pose.position.y =
y;
1981 target.pose.position.z =
z;
1989 geometry_msgs::PoseStamped target;
1997 target.pose.position.x = 0.0;
1998 target.pose.position.y = 0.0;
1999 target.pose.position.z = 0.0;
2011 const std::string& end_effector_link)
2013 geometry_msgs::PoseStamped target;
2021 target.pose.position.x = 0.0;
2022 target.pose.position.y = 0.0;
2023 target.pose.position.z = 0.0;
2027 target.pose.orientation.x =
x;
2028 target.pose.orientation.y =
y;
2029 target.pose.orientation.z =
z;
2030 target.pose.orientation.w = w;
2095 moveit::core::RobotStatePtr current_state;
2096 std::vector<double>
values;
2098 current_state->copyJointGroupPositions(
getName(), values);
2104 std::vector<double>
r;
2111 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2112 Eigen::Isometry3d pose;
2118 moveit::core::RobotStatePtr current_state;
2124 pose = current_state->getGlobalLinkTransform(lm);
2127 geometry_msgs::PoseStamped pose_msg;
2137 Eigen::Isometry3d pose;
2143 moveit::core::RobotStatePtr current_state;
2148 pose = current_state->getGlobalLinkTransform(lm);
2151 geometry_msgs::PoseStamped pose_msg;
2160 std::vector<double> result;
2166 moveit::core::RobotStatePtr current_state;
2173 geometry_msgs::TransformStamped tfs =
tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2174 double pitch, roll, yaw;
2175 tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2202 moveit::core::RobotStatePtr current_state;
2204 return current_state;
2347 return attachObject(
object, link, std::vector<std::string>());
2351 const std::vector<std::string>& touch_links)