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);
132 std::string desc = opt.robot_description_.length() ? opt.robot_description_ :
ROBOT_DESCRIPTION;
134 std::string kinematics_desc = desc +
"_kinematics/";
139 node_handle_.
param<
double>(kinematics_desc + opt.group_name_ +
"/goal_orientation_tolerance",
142 std::string planning_desc = desc +
"_planning/";
162 double allotted_time = wait_for_servers.
toSec();
164 move_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>>(
168 pick_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::PickupAction>>(
172 place_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::PlaceAction>>(
176 execute_action_client_ = std::make_unique<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>>(
195 template <
typename T>
238 if (!action->isServerConnected())
240 std::stringstream error;
241 error <<
"Unable to connect to move_group action server '" <<
name <<
"' within allotted time (" << allotted_time
243 throw std::runtime_error(error.str());
257 const std::shared_ptr<tf2_ros::Buffer>&
getTF()
const
267 const moveit::core::RobotModelConstPtr&
getRobotModel()
const
284 moveit_msgs::QueryPlannerInterfaces::Request req;
285 moveit_msgs::QueryPlannerInterfaces::Response
res;
287 if (!
res.planner_interfaces.empty())
289 desc =
res.planner_interfaces.front();
297 moveit_msgs::QueryPlannerInterfaces::Request req;
298 moveit_msgs::QueryPlannerInterfaces::Response
res;
300 if (!
res.planner_interfaces.empty())
302 desc =
res.planner_interfaces;
308 std::map<std::string, std::string>
getPlannerParams(
const std::string& planner_id,
const std::string&
group =
"")
310 moveit_msgs::GetPlannerParams::Request req;
311 moveit_msgs::GetPlannerParams::Response
res;
312 req.planner_config = planner_id;
314 std::map<std::string, std::string> result;
317 for (
unsigned int i = 0, end =
res.params.keys.size(); i < end; ++i)
318 result[
res.params.keys[i]] =
res.params.values[i];
324 const std::map<std::string, std::string>& params,
bool replace =
false)
326 moveit_msgs::SetPlannerParams::Request req;
327 moveit_msgs::SetPlannerParams::Response
res;
328 req.planner_config = planner_id;
330 req.replace = replace;
331 for (
const std::pair<const std::string, std::string>& param : params)
333 req.params.keys.push_back(
param.first);
334 req.params.values.push_back(
param.second);
341 std::string default_planning_pipeline;
343 return default_planning_pipeline;
369 std::stringstream param_name;
370 param_name <<
"move_group";
371 if (!pipeline_id.empty())
372 param_name <<
"/planning_pipelines/" << pipeline_id;
374 param_name <<
"/" <<
group;
375 param_name <<
"/default_planner_config";
377 std::string default_planner_config;
379 return default_planner_config;
407 void setMaxScalingFactor(
double& variable,
const double target_value,
const char* factor_name,
double fallback_value)
409 if (target_value > 1.0)
414 else if (target_value <= 0.0)
416 node_handle_.
param<
double>(std::string(
"robot_description_planning/default_") + factor_name, variable,
418 if (target_value < 0.0)
425 variable = target_value;
467 moveit::core::RobotStatePtr
s;
473 bool setJointValueTarget(
const geometry_msgs::Pose& eef_pose,
const std::string& end_effector_link,
474 const std::string& frame,
bool approx)
476 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
549 const std::vector<std::string>& possible_eefs =
551 for (
const std::string& possible_eef : possible_eefs)
555 static std::string empty;
559 bool setPoseTargets(
const std::vector<geometry_msgs::PoseStamped>& poses,
const std::string& end_effector_link)
561 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
571 std::vector<geometry_msgs::PoseStamped>& stored_poses =
pose_targets_[eef];
572 for (geometry_msgs::PoseStamped& stored_pose : stored_poses)
578 bool hasPoseTarget(
const std::string& end_effector_link)
const
580 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
584 const geometry_msgs::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const
586 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
589 std::map<std::string, std::vector<geometry_msgs::PoseStamped>>::const_iterator jt =
pose_targets_.find(eef);
591 if (!jt->second.empty())
592 return jt->second.at(0);
595 static const geometry_msgs::PoseStamped UNKNOWN;
600 const std::vector<geometry_msgs::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const
602 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
604 std::map<std::string, std::vector<geometry_msgs::PoseStamped>>::const_iterator jt =
pose_targets_.find(eef);
606 if (!jt->second.empty())
610 static const std::vector<geometry_msgs::PoseStamped> EMPTY;
656 bool getCurrentState(moveit::core::RobotStatePtr& current_state,
double wait_seconds = 1.0)
679 std::vector<moveit_msgs::PlaceLocation>
682 std::vector<moveit_msgs::PlaceLocation> locations;
683 for (
const geometry_msgs::PoseStamped& pose : poses)
685 moveit_msgs::PlaceLocation location;
686 location.pre_place_approach.direction.vector.z = -1.0;
687 location.post_place_retreat.direction.vector.x = -1.0;
688 location.pre_place_approach.direction.header.frame_id =
getRobotModel()->getModelFrame();
691 location.pre_place_approach.min_distance = 0.1;
692 location.pre_place_approach.desired_distance = 0.2;
693 location.post_place_retreat.min_distance = 0.0;
694 location.post_place_retreat.desired_distance = 0.2;
697 location.place_pose = pose;
698 locations.push_back(location);
709 return moveit::core::MoveItErrorCode::FAILURE;
714 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
740 return moveit::core::MoveItErrorCode::FAILURE;
745 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
773 std::map<std::string, moveit_msgs::CollisionObject> objects = psi.
getObjects(std::vector<std::string>(1,
object));
778 "Asked for grasps for the object '" <<
object <<
"', but the object could not be found");
779 return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME;
791 <<
"' is not available."
792 " This has to be implemented and started separately.");
793 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
796 moveit_msgs::GraspPlanning::Request request;
797 moveit_msgs::GraspPlanning::Response
response;
800 request.target = object;
805 response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
808 return moveit::core::MoveItErrorCode::FAILURE;
819 return moveit::core::MoveItErrorCode::FAILURE;
824 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
827 moveit_msgs::MoveGroupGoal goal;
829 goal.planning_options.plan_only =
true;
830 goal.planning_options.look_around =
false;
831 goal.planning_options.replan =
false;
832 goal.planning_options.planning_scene_diff.is_diff =
true;
833 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
860 return moveit::core::MoveItErrorCode::FAILURE;
865 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
868 moveit_msgs::MoveGroupGoal goal;
870 goal.planning_options.plan_only =
false;
871 goal.planning_options.look_around =
can_look_;
874 goal.planning_options.planning_scene_diff.is_diff =
true;
875 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
880 return moveit::core::MoveItErrorCode::SUCCESS;
905 return moveit::core::MoveItErrorCode::FAILURE;
910 return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
913 moveit_msgs::ExecuteTrajectoryGoal goal;
914 goal.trajectory = trajectory;
919 return moveit::core::MoveItErrorCode::SUCCESS;
939 double computeCartesianPath(
const std::vector<geometry_msgs::Pose>& waypoints,
double step,
double jump_threshold,
940 moveit_msgs::RobotTrajectory& msg,
const moveit_msgs::Constraints& path_constraints,
941 bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
943 moveit_msgs::GetCartesianPath::Request req;
944 moveit_msgs::GetCartesianPath::Response
res;
949 req.start_state.is_diff =
true;
954 req.waypoints = waypoints;
956 req.jump_threshold = jump_threshold;
957 req.path_constraints = path_constraints;
958 req.avoid_collisions = avoid_collisions;
965 error_code =
res.error_code;
966 if (
res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
976 error_code.val = error_code.FAILURE;
985 std_msgs::String event;
991 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
1005 moveit_msgs::AttachedCollisionObject aco;
1006 aco.object.id = object;
1007 aco.link_name.swap(l);
1008 if (touch_links.empty())
1009 aco.touch_links.push_back(aco.link_name);
1011 aco.touch_links = touch_links;
1012 aco.object.operation = moveit_msgs::CollisionObject::ADD;
1019 moveit_msgs::AttachedCollisionObject aco;
1022 aco.link_name =
name;
1024 aco.object.id =
name;
1025 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1026 if (aco.link_name.empty() && aco.object.id.empty())
1030 for (
const std::string& lname : lnames)
1032 aco.link_name = lname;
1098 request.start_state.is_diff =
true;
1102 request.goal_constraints.resize(1);
1109 std::size_t goal_count = 0;
1111 goal_count = std::max(goal_count, pose_target.second.size());
1117 request.goal_constraints.resize(goal_count);
1121 for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1126 c.position_constraints.clear();
1128 c.orientation_constraints.clear();
1147 moveit_msgs::PickupGoal
constructPickupGoal(
const std::string&
object, std::vector<moveit_msgs::Grasp>&& grasps,
1148 bool plan_only =
false)
const
1150 moveit_msgs::PickupGoal goal;
1151 goal.target_name = object;
1155 goal.possible_grasps = std::move(grasps);
1157 goal.allow_gripper_support_collision =
true;
1165 goal.planning_options.plan_only = plan_only;
1166 goal.planning_options.look_around =
can_look_;
1169 goal.planning_options.planning_scene_diff.is_diff =
true;
1170 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1175 std::vector<moveit_msgs::PlaceLocation>&& locations,
1176 bool plan_only =
false)
const
1178 moveit_msgs::PlaceGoal goal;
1180 goal.attached_object_name = object;
1182 goal.place_locations = std::move(locations);
1184 goal.allow_gripper_support_collision =
true;
1192 goal.planning_options.plan_only = plan_only;
1193 goal.planning_options.look_around =
can_look_;
1196 goal.planning_options.planning_scene_diff.is_diff =
true;
1197 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1213 path_constraints_ = std::make_unique<moveit_msgs::Constraints>(
static_cast<moveit_msgs::Constraints
>(*msg_m));
1246 std::vector<std::string>
c;
1258 return moveit_msgs::Constraints();
1266 return moveit_msgs::TrajectoryConstraints();
1278 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
1297 conn->setParams(host, port);
1298 if (conn->connect())
1303 catch (std::exception& ex)
1315 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>>
move_action_client_;
1316 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>>
execute_action_client_;
1317 std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction>>
pick_action_client_;
1373 throw std::runtime_error(
"ROS does not seem to be running");
1390 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1402 : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1404 other.impl_ =
nullptr;
1412 impl_ = other.
impl_;
1413 remembered_joint_values_ = std::move(other.remembered_joint_values_);
1414 other.impl_ =
nullptr;
1453 const std::string&
group)
const
1459 const std::map<std::string, std::string>& params,
bool replace)
1560 std::vector<moveit_msgs::Grasp> grasps,
1561 bool plan_only =
false)
const
1567 std::vector<moveit_msgs::PlaceLocation> locations,
1568 bool plan_only =
false)
const
1573 std::vector<moveit_msgs::PlaceLocation>
1601 double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1602 bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1604 return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::Constraints(),
1605 avoid_collisions, error_code);
1609 double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1610 const moveit_msgs::Constraints& path_constraints,
bool avoid_collisions,
1611 moveit_msgs::MoveItErrorCodes* error_code)
1613 moveit_msgs::MoveItErrorCodes err_tmp;
1614 moveit_msgs::MoveItErrorCodes& err = error_code ? *error_code : err_tmp;
1616 avoid_collisions, err);
1626 moveit::core::RobotStatePtr rs;
1627 if (start_state.is_diff)
1631 rs = std::make_shared<moveit::core::RobotState>(
getRobotModel());
1632 rs->setToDefaultValues();
1667 std::map<std::string, double> positions;
1672 for (
size_t x = 0;
x < names.size(); ++
x)
1674 positions[names[
x]] = it->second[
x];
1711 if (joint_values.size() != n_group_joints)
1715 <<
" has " << n_group_joints <<
" joints");
1726 for (
const auto& pair : variable_values)
1728 if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1730 ROS_ERROR_STREAM(
"joint variable " << pair.first <<
" is not part of group "
1742 const std::vector<double>& variable_values)
1744 if (variable_names.size() != variable_values.size())
1750 for (
const auto& variable_name : variable_names)
1752 if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1754 ROS_ERROR_STREAM(
"joint variable " << variable_name <<
" is not part of group "
1803 const std::string& end_effector_link)
1810 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1815 const std::string& end_effector_link)
1821 const std::string& end_effector_link)
1827 const std::string& end_effector_link)
1829 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1882 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1891 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1892 pose_msg[0].pose = target;
1900 std::vector<geometry_msgs::PoseStamped> targets(1, target);
1906 std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1909 for (std::size_t i = 0; i < target.size(); ++i)
1912 pose_out[i].header.stamp = tm;
1913 pose_out[i].header.frame_id = frame_id;
1919 const std::string& end_effector_link)
1921 std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1924 for (std::size_t i = 0; i < target.size(); ++i)
1926 target_stamped[i].pose = target[i];
1927 target_stamped[i].header.stamp = tm;
1928 target_stamped[i].header.frame_id = frame_id;
1934 const std::string& end_effector_link)
1953 const std::vector<geometry_msgs::PoseStamped>&
1962 geometry_msgs::PoseStamped& target)
1964 if (desired_frame != target.header.frame_id)
1966 geometry_msgs::PoseStamped target_in(target);
1976 geometry_msgs::PoseStamped target;
1984 target.pose.orientation.x = 0.0;
1985 target.pose.orientation.y = 0.0;
1986 target.pose.orientation.z = 0.0;
1987 target.pose.orientation.w = 1.0;
1991 target.pose.position.x =
x;
1992 target.pose.position.y =
y;
1993 target.pose.position.z =
z;
2001 geometry_msgs::PoseStamped target;
2009 target.pose.position.x = 0.0;
2010 target.pose.position.y = 0.0;
2011 target.pose.position.z = 0.0;
2023 const std::string& end_effector_link)
2025 geometry_msgs::PoseStamped target;
2033 target.pose.position.x = 0.0;
2034 target.pose.position.y = 0.0;
2035 target.pose.position.z = 0.0;
2039 target.pose.orientation.x =
x;
2040 target.pose.orientation.y =
y;
2041 target.pose.orientation.z =
z;
2042 target.pose.orientation.w = w;
2107 moveit::core::RobotStatePtr current_state;
2108 std::vector<double>
values;
2110 current_state->copyJointGroupPositions(
getName(), values);
2116 std::vector<double>
r;
2123 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2124 Eigen::Isometry3d pose;
2130 moveit::core::RobotStatePtr current_state;
2136 pose = current_state->getGlobalLinkTransform(lm);
2139 geometry_msgs::PoseStamped pose_msg;
2149 Eigen::Isometry3d pose;
2155 moveit::core::RobotStatePtr current_state;
2160 pose = current_state->getGlobalLinkTransform(lm);
2163 geometry_msgs::PoseStamped pose_msg;
2172 std::vector<double> result;
2178 moveit::core::RobotStatePtr current_state;
2185 geometry_msgs::TransformStamped tfs =
tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2186 double pitch, roll, yaw;
2187 tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2214 moveit::core::RobotStatePtr current_state;
2216 return current_state;
2359 return attachObject(
object, link, std::vector<std::string>());
2363 const std::vector<std::string>& touch_links)