00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <stdexcept>
00040 #include <moveit/warehouse/constraints_storage.h>
00041 #include <moveit/kinematic_constraints/utils.h>
00042 #include <moveit/move_group/capability_names.h>
00043 #include <moveit/move_group_pick_place_capability/capability_names.h>
00044 #include <moveit/move_group_interface/move_group.h>
00045 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00046 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00047 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00048 #include <moveit/common_planning_interface_objects/common_objects.h>
00049 #include <moveit/robot_state/conversions.h>
00050 #include <moveit_msgs/MoveGroupAction.h>
00051 #include <moveit_msgs/PickupAction.h>
00052 #include <moveit_msgs/PlaceAction.h>
00053 #include <moveit_msgs/ExecuteKnownTrajectory.h>
00054 #include <moveit_msgs/QueryPlannerInterfaces.h>
00055 #include <moveit_msgs/GetCartesianPath.h>
00056
00057 #include <actionlib/client/simple_action_client.h>
00058 #include <eigen_conversions/eigen_msg.h>
00059 #include <std_msgs/String.h>
00060 #include <tf/transform_listener.h>
00061 #include <tf/transform_datatypes.h>
00062 #include <tf_conversions/tf_eigen.h>
00063 #include <ros/console.h>
00064 #include <ros/ros.h>
00065
00066 namespace moveit
00067 {
00068 namespace planning_interface
00069 {
00070
00071 const std::string MoveGroup::ROBOT_DESCRIPTION = "robot_description";
00072
00073 namespace
00074 {
00075
00076 enum ActiveTargetType
00077 {
00078 JOINT, POSE, POSITION, ORIENTATION
00079 };
00080
00081 }
00082
00083 class MoveGroup::MoveGroupImpl
00084 {
00085 public:
00086
00087 MoveGroupImpl(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
00088 : opt_(opt),
00089 node_handle_(opt.node_handle_),
00090 tf_(tf)
00091 {
00092 robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
00093 if (!getRobotModel())
00094 {
00095 std::string error = "Unable to construct robot model. Please make sure all needed information is on the parameter server.";
00096 ROS_FATAL_STREAM(error);
00097 throw std::runtime_error(error);
00098 }
00099
00100 if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
00101 {
00102 std::string error = "Group '" + opt.group_name_ + "' was not found.";
00103 ROS_FATAL_STREAM(error);
00104 throw std::runtime_error(error);
00105 }
00106 joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
00107
00108 joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
00109 joint_state_target_->setToDefaultValues();
00110 active_target_ = JOINT;
00111 can_look_ = false;
00112 can_replan_ = false;
00113 replan_delay_ = 2.0;
00114 goal_joint_tolerance_ = 1e-4;
00115 goal_position_tolerance_ = 1e-4;
00116 goal_orientation_tolerance_ = 1e-3;
00117 planning_time_ = 5.0;
00118 num_planning_attempts_ = 1;
00119 initializing_constraints_ = false;
00120
00121 if (joint_model_group_->isChain())
00122 end_effector_link_ = joint_model_group_->getLinkModelNames().back();
00123 pose_reference_frame_ = getRobotModel()->getModelFrame();
00124
00125 trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);
00126 attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false);
00127
00128 current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_);
00129
00130 move_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_,
00131 move_group::MOVE_ACTION,
00132 false));
00133 waitForAction(move_action_client_, wait_for_server, move_group::MOVE_ACTION);
00134
00135 pick_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PickupAction>(node_handle_,
00136 move_group::PICKUP_ACTION,
00137 false));
00138 waitForAction(pick_action_client_, wait_for_server, move_group::PICKUP_ACTION);
00139
00140 place_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PlaceAction>(node_handle_,
00141 move_group::PLACE_ACTION,
00142 false));
00143 waitForAction(place_action_client_, wait_for_server, move_group::PLACE_ACTION);
00144
00145 execute_service_ = node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
00146 query_service_ = node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
00147 cartesian_path_service_ = node_handle_.serviceClient<moveit_msgs::GetCartesianPath>(move_group::CARTESIAN_PATH_SERVICE_NAME);
00148
00149 ROS_INFO_STREAM("Ready to take MoveGroup commands for group " << opt.group_name_ << ".");
00150 }
00151
00152 template<typename T>
00153 void waitForAction(const T &action, const ros::Duration &wait_for_server, const std::string &name)
00154 {
00155 ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
00156
00157
00158 ros::Time start_time = ros::Time::now();
00159 while (start_time == ros::Time::now())
00160 {
00161 ros::WallDuration(0.01).sleep();
00162 ros::spinOnce();
00163 }
00164
00165
00166 if (wait_for_server == ros::Duration(0, 0))
00167 {
00168 while (node_handle_.ok() && !action->isServerConnected())
00169 {
00170 ros::WallDuration(0.02).sleep();
00171 ros::spinOnce();
00172 }
00173 }
00174 else
00175 {
00176 ros::Time final_time = ros::Time::now() + wait_for_server;
00177 while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
00178 {
00179 ros::WallDuration(0.02).sleep();
00180 ros::spinOnce();
00181 }
00182 }
00183
00184 if (!action->isServerConnected())
00185 throw std::runtime_error("Unable to connect to move_group action server within allotted time (2)");
00186 else
00187 ROS_DEBUG("Connected to '%s'", name.c_str());
00188 }
00189
00190 ~MoveGroupImpl()
00191 {
00192 if (constraints_init_thread_)
00193 constraints_init_thread_->join();
00194 }
00195
00196 const boost::shared_ptr<tf::Transformer>& getTF() const
00197 {
00198 return tf_;
00199 }
00200
00201 const Options& getOptions() const
00202 {
00203 return opt_;
00204 }
00205
00206 const robot_model::RobotModelConstPtr& getRobotModel() const
00207 {
00208 return robot_model_;
00209 }
00210
00211 const robot_model::JointModelGroup* getJointModelGroup() const
00212 {
00213 return joint_model_group_;
00214 }
00215
00216 bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
00217 {
00218 moveit_msgs::QueryPlannerInterfaces::Request req;
00219 moveit_msgs::QueryPlannerInterfaces::Response res;
00220 if (query_service_.call(req, res))
00221 if (!res.planner_interfaces.empty())
00222 {
00223 desc = res.planner_interfaces.front();
00224 return true;
00225 }
00226 return false;
00227 }
00228
00229 void setPlannerId(const std::string &planner_id)
00230 {
00231 planner_id_ = planner_id;
00232 }
00233
00234 void setNumPlanningAttempts(unsigned int num_planning_attempts)
00235 {
00236 num_planning_attempts_ = num_planning_attempts;
00237 }
00238
00239 robot_state::RobotState& getJointStateTarget()
00240 {
00241 return *joint_state_target_;
00242 }
00243
00244 void setStartState(const robot_state::RobotState &start_state)
00245 {
00246 considered_start_state_.reset(new robot_state::RobotState(start_state));
00247 }
00248
00249 void setStartStateToCurrentState()
00250 {
00251 considered_start_state_.reset();
00252 }
00253
00254 robot_state::RobotStatePtr getStartState()
00255 {
00256 if (considered_start_state_)
00257 return considered_start_state_;
00258 else
00259 {
00260 robot_state::RobotStatePtr s;
00261 getCurrentState(s);
00262 return s;
00263 }
00264 }
00265
00266 bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
00267 {
00268 const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
00269
00270 if (!eef.empty())
00271 {
00272
00273 moveit::core::RobotStatePtr c = getStartState();
00274 if (c)
00275 {
00276 setTargetType(JOINT);
00277 c->enforceBounds();
00278 getJointStateTarget() = *c;
00279 if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
00280 return false;
00281 }
00282 else
00283 return false;
00284
00285
00286 kinematics::KinematicsQueryOptions o;
00287 o.return_approximate_solution = approx;
00288
00289
00290 if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
00291 return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0, moveit::core::GroupStateValidityCallbackFn(), o);
00292 else
00293 {
00294 if (c->knowsFrameTransform(frame))
00295 {
00296
00297 const Eigen::Affine3d &t = getJointStateTarget().getFrameTransform(frame);
00298 Eigen::Affine3d p;
00299 tf::poseMsgToEigen(eef_pose, p);
00300 return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0, moveit::core::GroupStateValidityCallbackFn(), o);
00301 }
00302 else
00303 {
00304 logError("Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str());
00305 return false;
00306 }
00307 }
00308 }
00309 else
00310 return false;
00311 }
00312
00313 void setEndEffectorLink(const std::string &end_effector)
00314 {
00315 end_effector_link_ = end_effector;
00316 }
00317
00318 void clearPoseTarget(const std::string &end_effector_link)
00319 {
00320 pose_targets_.erase(end_effector_link);
00321 }
00322
00323 void clearPoseTargets()
00324 {
00325 pose_targets_.clear();
00326 }
00327
00328 const std::string &getEndEffectorLink() const
00329 {
00330 return end_effector_link_;
00331 }
00332
00333 const std::string& getEndEffector() const
00334 {
00335 if (!end_effector_link_.empty())
00336 {
00337 const std::vector<std::string> &possible_eefs = getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
00338 for (std::size_t i = 0 ; i < possible_eefs.size() ; ++i)
00339 if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
00340 return possible_eefs[i];
00341 }
00342 static std::string empty;
00343 return empty;
00344 }
00345
00346 bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &poses, const std::string &end_effector_link)
00347 {
00348 const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00349 if (eef.empty())
00350 {
00351 ROS_ERROR("No end-effector to set the pose for");
00352 return false;
00353 }
00354 else
00355 {
00356 pose_targets_[eef] = poses;
00357
00358 std::vector<geometry_msgs::PoseStamped> &stored_poses = pose_targets_[eef];
00359 for (std::size_t i = 0 ; i < stored_poses.size() ; ++i)
00360 stored_poses[i].header.stamp = ros::Time(0);
00361 }
00362 return true;
00363 }
00364
00365 bool hasPoseTarget(const std::string &end_effector_link) const
00366 {
00367 const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00368 return pose_targets_.find(eef) != pose_targets_.end();
00369 }
00370
00371 const geometry_msgs::PoseStamped& getPoseTarget(const std::string &end_effector_link) const
00372 {
00373 const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00374
00375
00376 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00377 if (jt != pose_targets_.end())
00378 if (!jt->second.empty())
00379 return jt->second.at(0);
00380
00381
00382 static const geometry_msgs::PoseStamped unknown;
00383 ROS_ERROR("Pose for end effector '%s' not known.", eef.c_str());
00384 return unknown;
00385 }
00386
00387 const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string &end_effector_link) const
00388 {
00389 const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00390
00391 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00392 if (jt != pose_targets_.end())
00393 if (!jt->second.empty())
00394 return jt->second;
00395
00396
00397 static const std::vector<geometry_msgs::PoseStamped> empty;
00398 ROS_ERROR("Poses for end effector '%s' are not known.", eef.c_str());
00399 return empty;
00400 }
00401
00402 void setPoseReferenceFrame(const std::string &pose_reference_frame)
00403 {
00404 pose_reference_frame_ = pose_reference_frame;
00405 }
00406
00407 void setSupportSurfaceName(const std::string &support_surface)
00408 {
00409 support_surface_ = support_surface;
00410 }
00411
00412 const std::string& getPoseReferenceFrame() const
00413 {
00414 return pose_reference_frame_;
00415 }
00416
00417 void setTargetType(ActiveTargetType type)
00418 {
00419 active_target_ = type;
00420 }
00421
00422 ActiveTargetType getTargetType() const
00423 {
00424 return active_target_;
00425 }
00426
00427 bool startStateMonitor(double wait)
00428 {
00429 if (!current_state_monitor_)
00430 {
00431 ROS_ERROR("Unable to monitor current robot state");
00432 return false;
00433 }
00434
00435
00436 if (!current_state_monitor_->isActive())
00437 current_state_monitor_->startStateMonitor();
00438
00439 current_state_monitor_->waitForCurrentState(opt_.group_name_, wait);
00440 return true;
00441 }
00442
00443 bool getCurrentState(robot_state::RobotStatePtr ¤t_state, double wait_seconds = 1.0)
00444 {
00445 if (!current_state_monitor_)
00446 {
00447 ROS_ERROR("Unable to get current robot state");
00448 return false;
00449 }
00450
00451
00452 if (!current_state_monitor_->isActive())
00453 current_state_monitor_->startStateMonitor();
00454
00455 if (!current_state_monitor_->waitForCurrentState(opt_.group_name_, wait_seconds))
00456 ROS_WARN("Joint values for monitored state are requested but the full state is not known");
00457
00458 current_state = current_state_monitor_->getCurrentState();
00459 return true;
00460 }
00461
00463 MoveItErrorCode place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
00464 {
00465 std::vector<moveit_msgs::PlaceLocation> locations;
00466 for (std::size_t i = 0; i < poses.size(); ++i)
00467 {
00468 moveit_msgs::PlaceLocation location;
00469 location.pre_place_approach.direction.vector.z = -1.0;
00470 location.post_place_retreat.direction.vector.x = -1.0;
00471 location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
00472 location.post_place_retreat.direction.header.frame_id = end_effector_link_;
00473
00474 location.pre_place_approach.min_distance = 0.1;
00475 location.pre_place_approach.desired_distance = 0.2;
00476 location.post_place_retreat.min_distance = 0.0;
00477 location.post_place_retreat.desired_distance = 0.2;
00478
00479
00480 location.place_pose = poses[i];
00481 locations.push_back(location);
00482 }
00483 ROS_DEBUG("Move group interface has %u place locations", (unsigned int) locations.size());
00484 return place(object, locations);
00485 }
00486
00487 MoveItErrorCode place(const std::string &object, const std::vector<moveit_msgs::PlaceLocation> &locations)
00488 {
00489 if (!place_action_client_)
00490 {
00491 ROS_ERROR_STREAM("Place action client not found");
00492 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00493 }
00494 if (!place_action_client_->isServerConnected())
00495 {
00496 ROS_ERROR_STREAM("Place action server not connected");
00497 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00498 }
00499 moveit_msgs::PlaceGoal goal;
00500 constructGoal(goal, object);
00501 goal.place_locations = locations;
00502 goal.planning_options.plan_only = false;
00503 goal.planning_options.look_around = can_look_;
00504 goal.planning_options.replan = can_replan_;
00505 goal.planning_options.replan_delay = replan_delay_;
00506 goal.planning_options.planning_scene_diff.is_diff = true;
00507 goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00508
00509 place_action_client_->sendGoal(goal);
00510 ROS_DEBUG("Sent place goal with %d locations", (int) goal.place_locations.size());
00511 if (!place_action_client_->waitForResult())
00512 {
00513 ROS_INFO_STREAM("Place action returned early");
00514 }
00515 if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00516 {
00517 return MoveItErrorCode(place_action_client_->getResult()->error_code);
00518 }
00519 else
00520 {
00521 ROS_WARN_STREAM("Fail: " << place_action_client_->getState().toString() << ": " << place_action_client_->getState().getText());
00522 return MoveItErrorCode(place_action_client_->getResult()->error_code);
00523 }
00524 }
00525
00526 MoveItErrorCode pick(const std::string &object, const std::vector<moveit_msgs::Grasp> &grasps)
00527 {
00528 if (!pick_action_client_)
00529 {
00530 ROS_ERROR_STREAM("Pick action client not found");
00531 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00532 }
00533 if (!pick_action_client_->isServerConnected())
00534 {
00535 ROS_ERROR_STREAM("Pick action server not connected");
00536 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00537 }
00538 moveit_msgs::PickupGoal goal;
00539 constructGoal(goal, object);
00540 goal.possible_grasps = grasps;
00541 goal.planning_options.plan_only = false;
00542 goal.planning_options.look_around = can_look_;
00543 goal.planning_options.replan = can_replan_;
00544 goal.planning_options.replan_delay = replan_delay_;
00545 goal.planning_options.planning_scene_diff.is_diff = true;
00546 goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00547
00548 pick_action_client_->sendGoal(goal);
00549 if (!pick_action_client_->waitForResult())
00550 {
00551 ROS_INFO_STREAM("Pickup action returned early");
00552 }
00553 if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00554 {
00555 return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00556 }
00557 else
00558 {
00559 ROS_WARN_STREAM("Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText());
00560 return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00561 }
00562 }
00563
00564 MoveItErrorCode plan(Plan &plan)
00565 {
00566 if (!move_action_client_)
00567 {
00568 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00569 }
00570 if (!move_action_client_->isServerConnected())
00571 {
00572 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00573 }
00574
00575 moveit_msgs::MoveGroupGoal goal;
00576 constructGoal(goal);
00577 goal.planning_options.plan_only = true;
00578 goal.planning_options.look_around = false;
00579 goal.planning_options.replan = false;
00580 goal.planning_options.planning_scene_diff.is_diff = true;
00581 goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00582
00583 move_action_client_->sendGoal(goal);
00584 if (!move_action_client_->waitForResult())
00585 {
00586 ROS_INFO_STREAM("MoveGroup action returned early");
00587 }
00588 if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00589 {
00590 plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
00591 plan.start_state_ = move_action_client_->getResult()->trajectory_start;
00592 plan.planning_time_ = move_action_client_->getResult()->planning_time;
00593 return MoveItErrorCode(move_action_client_->getResult()->error_code);
00594 }
00595 else
00596 {
00597 ROS_WARN_STREAM("Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
00598 return MoveItErrorCode(move_action_client_->getResult()->error_code);
00599 }
00600 }
00601
00602 MoveItErrorCode move(bool wait)
00603 {
00604 if (!move_action_client_)
00605 {
00606 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00607 }
00608 if (!move_action_client_->isServerConnected())
00609 {
00610 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00611 }
00612
00613 moveit_msgs::MoveGroupGoal goal;
00614 constructGoal(goal);
00615 goal.planning_options.plan_only = false;
00616 goal.planning_options.look_around = can_look_;
00617 goal.planning_options.replan = can_replan_;
00618 goal.planning_options.replan_delay = replan_delay_;
00619 goal.planning_options.planning_scene_diff.is_diff = true;
00620 goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00621
00622 move_action_client_->sendGoal(goal);
00623 if (!wait)
00624 {
00625 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00626 }
00627
00628 if (!move_action_client_->waitForResult())
00629 {
00630 ROS_INFO_STREAM("MoveGroup action returned early");
00631 }
00632
00633 if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00634 {
00635 return MoveItErrorCode(move_action_client_->getResult()->error_code);
00636 }
00637 else
00638 {
00639 ROS_INFO_STREAM(move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
00640 return MoveItErrorCode(move_action_client_->getResult()->error_code);
00641 }
00642 }
00643
00644 MoveItErrorCode execute(const Plan &plan, bool wait)
00645 {
00646 moveit_msgs::ExecuteKnownTrajectory::Request req;
00647 moveit_msgs::ExecuteKnownTrajectory::Response res;
00648 req.trajectory = plan.trajectory_;
00649 req.wait_for_execution = wait;
00650 if (execute_service_.call(req, res))
00651 {
00652 return MoveItErrorCode(res.error_code);
00653 }
00654 else
00655 {
00656 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00657 }
00658 }
00659
00660 double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double step, double jump_threshold,
00661 moveit_msgs::RobotTrajectory &msg, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
00662 {
00663 moveit_msgs::GetCartesianPath::Request req;
00664 moveit_msgs::GetCartesianPath::Response res;
00665
00666 if (considered_start_state_)
00667 robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
00668 req.group_name = opt_.group_name_;
00669 req.header.frame_id = getPoseReferenceFrame();
00670 req.header.stamp = ros::Time::now();
00671 req.waypoints = waypoints;
00672 req.max_step = step;
00673 req.jump_threshold = jump_threshold;
00674 req.avoid_collisions = avoid_collisions;
00675
00676 if (cartesian_path_service_.call(req, res))
00677 {
00678 error_code = res.error_code;
00679 if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00680 {
00681 msg = res.solution;
00682 return res.fraction;
00683 }
00684 else
00685 return -1.0;
00686 }
00687 else
00688 {
00689 error_code.val = error_code.FAILURE;
00690 return -1.0;
00691 }
00692 }
00693
00694 void stop()
00695 {
00696 if (trajectory_event_publisher_)
00697 {
00698 std_msgs::String event;
00699 event.data = "stop";
00700 trajectory_event_publisher_.publish(event);
00701 }
00702 }
00703
00704 bool attachObject(const std::string &object, const std::string &link, const std::vector<std::string> &touch_links)
00705 {
00706 std::string l = link.empty() ? getEndEffectorLink() : link;
00707 if (l.empty())
00708 {
00709 const std::vector<std::string> &links = joint_model_group_->getLinkModelNames();
00710 if (!links.empty())
00711 l = links[0];
00712 }
00713 if (l.empty())
00714 {
00715 ROS_ERROR("No known link to attach object '%s' to", object.c_str());
00716 return false;
00717 }
00718 moveit_msgs::AttachedCollisionObject aco;
00719 aco.object.id = object;
00720 aco.link_name.swap(l);
00721 if (touch_links.empty())
00722 aco.touch_links.push_back(aco.link_name);
00723 else
00724 aco.touch_links = touch_links;
00725 aco.object.operation = moveit_msgs::CollisionObject::ADD;
00726 attached_object_publisher_.publish(aco);
00727 return true;
00728 }
00729
00730 bool detachObject(const std::string &name)
00731 {
00732 moveit_msgs::AttachedCollisionObject aco;
00733
00734 if (!name.empty() && joint_model_group_->hasLinkModel(name))
00735 aco.link_name = name;
00736 else
00737 aco.object.id = name;
00738 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00739 if (aco.link_name.empty() && aco.object.id.empty())
00740 {
00741
00742 const std::vector<std::string> &lnames = joint_model_group_->getLinkModelNames();
00743 for (std::size_t i = 0 ; i < lnames.size() ; ++i)
00744 {
00745 aco.link_name = lnames[i];
00746 attached_object_publisher_.publish(aco);
00747 }
00748 }
00749 else
00750 attached_object_publisher_.publish(aco);
00751 return true;
00752 }
00753
00754 double getGoalPositionTolerance() const
00755 {
00756 return goal_position_tolerance_;
00757 }
00758
00759 double getGoalOrientationTolerance() const
00760 {
00761 return goal_orientation_tolerance_;
00762 }
00763
00764 double getGoalJointTolerance() const
00765 {
00766 return goal_joint_tolerance_;
00767 }
00768
00769 void setGoalJointTolerance(double tolerance)
00770 {
00771 goal_joint_tolerance_ = tolerance;
00772 }
00773
00774 void setGoalPositionTolerance(double tolerance)
00775 {
00776 goal_position_tolerance_ = tolerance;
00777 }
00778
00779 void setGoalOrientationTolerance(double tolerance)
00780 {
00781 goal_orientation_tolerance_ = tolerance;
00782 }
00783
00784 void setPlanningTime(double seconds)
00785 {
00786 if (seconds > 0.0)
00787 planning_time_ = seconds;
00788 }
00789
00790 double getPlanningTime() const
00791 {
00792 return planning_time_;
00793 }
00794
00795 void allowLooking(bool flag)
00796 {
00797 can_look_ = flag;
00798 ROS_INFO("Looking around: %s", can_look_ ? "yes" : "no");
00799 }
00800
00801 void allowReplanning(bool flag)
00802 {
00803 can_replan_ = flag;
00804 ROS_INFO("Replanning: %s", can_replan_ ? "yes" : "no");
00805 }
00806
00807 void setReplanningDelay(double delay)
00808 {
00809 if (delay >= 0.0)
00810 replan_delay_ = delay;
00811 }
00812
00813 double getReplanningDelay() const
00814 {
00815 return replan_delay_;
00816 }
00817
00818 void constructGoal(moveit_msgs::MoveGroupGoal &goal_out)
00819 {
00820 moveit_msgs::MoveGroupGoal goal;
00821 goal.request.group_name = opt_.group_name_;
00822 goal.request.num_planning_attempts = num_planning_attempts_;
00823 goal.request.allowed_planning_time = planning_time_;
00824 goal.request.planner_id = planner_id_;
00825 goal.request.workspace_parameters = workspace_parameters_;
00826 goal.request.start_state.is_diff = true;
00827
00828 if (considered_start_state_)
00829 robot_state::robotStateToRobotStateMsg(*considered_start_state_, goal.request.start_state);
00830
00831 if (active_target_ == JOINT)
00832 {
00833 goal.request.goal_constraints.resize(1);
00834 goal.request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(getJointStateTarget(), joint_model_group_, goal_joint_tolerance_);
00835 }
00836 else
00837 if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
00838 {
00839
00840 std::size_t goal_count = 0;
00841 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
00842 it != pose_targets_.end() ; ++it)
00843 goal_count = std::max(goal_count, it->second.size());
00844
00845
00846
00847
00848
00849 goal.request.goal_constraints.resize(goal_count);
00850
00851 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
00852 it != pose_targets_.end() ; ++it)
00853 {
00854 for (std::size_t i = 0 ; i < it->second.size() ; ++i)
00855 {
00856 moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
00857 if (active_target_ == ORIENTATION)
00858 c.position_constraints.clear();
00859 if (active_target_ == POSITION)
00860 c.orientation_constraints.clear();
00861 goal.request.goal_constraints[i] = kinematic_constraints::mergeConstraints(goal.request.goal_constraints[i], c);
00862 }
00863 }
00864 }
00865 else
00866 ROS_ERROR("Unable to construct goal representation");
00867
00868 if (path_constraints_)
00869 goal.request.path_constraints = *path_constraints_;
00870 goal_out = goal;
00871 }
00872
00873 void constructGoal(moveit_msgs::PickupGoal &goal_out, const std::string &object)
00874 {
00875 moveit_msgs::PickupGoal goal;
00876 goal.target_name = object;
00877 goal.group_name = opt_.group_name_;
00878 goal.end_effector = getEndEffector();
00879 goal.allowed_planning_time = planning_time_;
00880 goal.support_surface_name = support_surface_;
00881 goal.planner_id = planner_id_;
00882 if(!support_surface_.empty())
00883 goal.allow_gripper_support_collision = true;
00884
00885 if (path_constraints_)
00886 goal.path_constraints = *path_constraints_;
00887
00888 goal_out = goal;
00889 }
00890
00891 void constructGoal(moveit_msgs::PlaceGoal &goal_out, const std::string &object)
00892 {
00893 moveit_msgs::PlaceGoal goal;
00894 goal.attached_object_name = object;
00895 goal.group_name = opt_.group_name_;
00896 goal.allowed_planning_time = planning_time_;
00897 goal.support_surface_name = support_surface_;
00898 goal.planner_id = planner_id_;
00899 if(!support_surface_.empty())
00900 goal.allow_gripper_support_collision = true;
00901
00902 if (path_constraints_)
00903 goal.path_constraints = *path_constraints_;
00904
00905 goal_out = goal;
00906 }
00907
00908 void setPathConstraints(const moveit_msgs::Constraints &constraint)
00909 {
00910 path_constraints_.reset(new moveit_msgs::Constraints(constraint));
00911 }
00912
00913 bool setPathConstraints(const std::string &constraint)
00914 {
00915 if (constraints_storage_)
00916 {
00917 moveit_warehouse::ConstraintsWithMetadata msg_m;
00918 if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
00919 {
00920 path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
00921 return true;
00922 }
00923 else
00924 return false;
00925 }
00926 else
00927 return false;
00928 }
00929
00930 void clearPathConstraints()
00931 {
00932 path_constraints_.reset();
00933 }
00934
00935 std::vector<std::string> getKnownConstraints() const
00936 {
00937 while (initializing_constraints_)
00938 {
00939 static ros::WallDuration d(0.01);
00940 d.sleep();
00941 }
00942
00943 std::vector<std::string> c;
00944 if (constraints_storage_)
00945 constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
00946
00947 return c;
00948 }
00949
00950 moveit_msgs::Constraints getPathConstraints() const
00951 {
00952 if (path_constraints_)
00953 return *path_constraints_;
00954 else
00955 return moveit_msgs::Constraints();
00956 }
00957
00958 void initializeConstraintsStorage(const std::string &host, unsigned int port)
00959 {
00960 initializing_constraints_ = true;
00961 if (constraints_init_thread_)
00962 constraints_init_thread_->join();
00963 constraints_init_thread_.reset(new boost::thread(boost::bind(&MoveGroupImpl::initializeConstraintsStorageThread, this, host, port)));
00964 }
00965
00966 void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
00967 {
00968 workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
00969 workspace_parameters_.header.stamp = ros::Time::now();
00970 workspace_parameters_.min_corner.x = minx;
00971 workspace_parameters_.min_corner.y = miny;
00972 workspace_parameters_.min_corner.z = minz;
00973 workspace_parameters_.max_corner.x = maxx;
00974 workspace_parameters_.max_corner.y = maxy;
00975 workspace_parameters_.max_corner.z = maxz;
00976 }
00977
00978 private:
00979
00980 void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
00981 {
00982 try
00983 {
00984 constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(host, port));
00985 ROS_DEBUG("Connected to constraints database");
00986 }
00987 catch(std::runtime_error &ex)
00988 {
00989 ROS_DEBUG("%s", ex.what());
00990 }
00991 initializing_constraints_ = false;
00992 }
00993
00994 Options opt_;
00995 ros::NodeHandle node_handle_;
00996 boost::shared_ptr<tf::Transformer> tf_;
00997 robot_model::RobotModelConstPtr robot_model_;
00998 planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
00999 boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
01000 boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
01001 boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
01002
01003
01004 robot_state::RobotStatePtr considered_start_state_;
01005 moveit_msgs::WorkspaceParameters workspace_parameters_;
01006 double planning_time_;
01007 std::string planner_id_;
01008 unsigned int num_planning_attempts_;
01009 double goal_joint_tolerance_;
01010 double goal_position_tolerance_;
01011 double goal_orientation_tolerance_;
01012 bool can_look_;
01013 bool can_replan_;
01014 double replan_delay_;
01015
01016
01017 robot_state::RobotStatePtr joint_state_target_;
01018 const robot_model::JointModelGroup *joint_model_group_;
01019
01020
01021
01022 std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
01023
01024
01025 ActiveTargetType active_target_;
01026 boost::scoped_ptr<moveit_msgs::Constraints> path_constraints_;
01027 std::string end_effector_link_;
01028 std::string pose_reference_frame_;
01029 std::string support_surface_;
01030
01031
01032 ros::Publisher trajectory_event_publisher_;
01033 ros::Publisher attached_object_publisher_;
01034 ros::ServiceClient execute_service_;
01035 ros::ServiceClient query_service_;
01036 ros::ServiceClient cartesian_path_service_;
01037 boost::scoped_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
01038 boost::scoped_ptr<boost::thread> constraints_init_thread_;
01039 bool initializing_constraints_;
01040 };
01041 }
01042 }
01043
01044 moveit::planning_interface::MoveGroup::MoveGroup(const std::string &group_name, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
01045 {
01046 if (!ros::ok())
01047 throw std::runtime_error("ROS does not seem to be running");
01048 impl_ = new MoveGroupImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_server);
01049 }
01050
01051 moveit::planning_interface::MoveGroup::MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
01052 {
01053 impl_ = new MoveGroupImpl(opt, tf ? tf : getSharedTF(), wait_for_server);
01054 }
01055
01056 moveit::planning_interface::MoveGroup::~MoveGroup()
01057 {
01058 delete impl_;
01059 }
01060
01061 const std::string& moveit::planning_interface::MoveGroup::getName() const
01062 {
01063 return impl_->getOptions().group_name_;
01064 }
01065
01066 bool moveit::planning_interface::MoveGroup::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
01067 {
01068 return impl_->getInterfaceDescription(desc);
01069 }
01070
01071 void moveit::planning_interface::MoveGroup::setPlannerId(const std::string &planner_id)
01072 {
01073 impl_->setPlannerId(planner_id);
01074 }
01075
01076 void moveit::planning_interface::MoveGroup::setNumPlanningAttempts(unsigned int num_planning_attempts)
01077 {
01078 impl_->setNumPlanningAttempts(num_planning_attempts);
01079 }
01080
01081 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
01082 {
01083 return impl_->move(false);
01084 }
01085
01086 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::move()
01087 {
01088 return impl_->move(true);
01089 }
01090
01091 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncExecute(const Plan &plan)
01092 {
01093 return impl_->execute(plan, false);
01094 }
01095
01096 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::execute(const Plan &plan)
01097 {
01098 return impl_->execute(plan, true);
01099 }
01100
01101 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan &plan)
01102 {
01103 return impl_->plan(plan);
01104 }
01105
01106 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object)
01107 {
01108 return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
01109 }
01110
01111 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object, const moveit_msgs::Grasp &grasp)
01112 {
01113 return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
01114 }
01115
01116 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object, const std::vector<moveit_msgs::Grasp> &grasps)
01117 {
01118 return impl_->pick(object, grasps);
01119 }
01120
01121 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object)
01122 {
01123 return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
01124 }
01125
01126 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const std::vector<moveit_msgs::PlaceLocation> &locations)
01127 {
01128 return impl_->place(object, locations);
01129 }
01130
01131 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
01132 {
01133 return impl_->place(object, poses);
01134 }
01135
01136 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const geometry_msgs::PoseStamped &pose)
01137 {
01138 return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
01139 }
01140
01141 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double eef_step, double jump_threshold,
01142 moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions,
01143 moveit_msgs::MoveItErrorCodes *error_code)
01144 {
01145 if(error_code)
01146 {
01147 return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, avoid_collisions, *error_code);
01148 }
01149 else
01150 {
01151 moveit_msgs::MoveItErrorCodes error_code_tmp;
01152 return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, avoid_collisions, error_code_tmp);
01153 }
01154 }
01155
01156 void moveit::planning_interface::MoveGroup::stop()
01157 {
01158 impl_->stop();
01159 }
01160
01161 void moveit::planning_interface::MoveGroup::setStartState(const moveit_msgs::RobotState &start_state)
01162 {
01163 robot_state::RobotStatePtr rs;
01164 impl_->getCurrentState(rs);
01165 robot_state::robotStateMsgToRobotState(start_state, *rs);
01166 setStartState(*rs);
01167 }
01168
01169 void moveit::planning_interface::MoveGroup::setStartState(const robot_state::RobotState &start_state)
01170 {
01171 impl_->setStartState(start_state);
01172 }
01173
01174 void moveit::planning_interface::MoveGroup::setStartStateToCurrentState()
01175 {
01176 impl_->setStartStateToCurrentState();
01177 }
01178
01179 void moveit::planning_interface::MoveGroup::setRandomTarget()
01180 {
01181 impl_->getJointStateTarget().setToRandomPositions();
01182 impl_->setTargetType(JOINT);
01183 }
01184
01185 bool moveit::planning_interface::MoveGroup::setNamedTarget(const std::string &name)
01186 {
01187 std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01188 if (it != remembered_joint_values_.end())
01189 {
01190 return setJointValueTarget(it->second);
01191 }
01192 else
01193 {
01194 if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
01195 {
01196 impl_->setTargetType(JOINT);
01197 return true;
01198 }
01199 return false;
01200 }
01201 }
01202
01203 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::vector<double> &joint_values)
01204 {
01205 if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
01206 return false;
01207 impl_->setTargetType(JOINT);
01208 impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
01209 return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());
01210 }
01211
01212 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::map<std::string, double> &joint_values)
01213 {
01214 impl_->setTargetType(JOINT);
01215 impl_->getJointStateTarget().setVariablePositions(joint_values);
01216 return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01217 }
01218
01219 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const robot_state::RobotState &rstate)
01220 {
01221 impl_->setTargetType(JOINT);
01222 impl_->getJointStateTarget() = rstate;
01223 return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01224 }
01225
01226 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string &joint_name, double value)
01227 {
01228 std::vector<double> values(1, value);
01229 return setJointValueTarget(joint_name, values);
01230 }
01231
01232 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string &joint_name, const std::vector<double> &values)
01233 {
01234 impl_->setTargetType(JOINT);
01235 const robot_model::JointModel *jm = impl_->getJointStateTarget().getJointModel(joint_name);
01236 if (jm && jm->getVariableCount() == values.size())
01237 {
01238 impl_->getJointStateTarget().setJointPositions(jm, values);
01239 return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
01240 }
01241 return false;
01242 }
01243
01244 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const sensor_msgs::JointState &state)
01245 {
01246 impl_->setTargetType(JOINT);
01247 impl_->getJointStateTarget().setVariableValues(state);
01248 return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01249 }
01250
01251 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link)
01252 {
01253 return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
01254 }
01255
01256 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link)
01257 {
01258 return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
01259 }
01260
01261 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link)
01262 {
01263 geometry_msgs::Pose msg;
01264 tf::poseEigenToMsg(eef_pose, msg);
01265 return setJointValueTarget(msg, end_effector_link);
01266 }
01267
01268 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link)
01269 {
01270 return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
01271 }
01272
01273 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link)
01274 {
01275 return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
01276 }
01277
01278 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link)
01279 {
01280 geometry_msgs::Pose msg;
01281 tf::poseEigenToMsg(eef_pose, msg);
01282 return setApproximateJointValueTarget(msg, end_effector_link);
01283 }
01284
01285 const robot_state::RobotState& moveit::planning_interface::MoveGroup::getJointValueTarget() const
01286 {
01287 return impl_->getJointStateTarget();
01288 }
01289
01290 const std::string& moveit::planning_interface::MoveGroup::getEndEffectorLink() const
01291 {
01292 return impl_->getEndEffectorLink();
01293 }
01294
01295 const std::string& moveit::planning_interface::MoveGroup::getEndEffector() const
01296 {
01297 return impl_->getEndEffector();
01298 }
01299
01300 bool moveit::planning_interface::MoveGroup::setEndEffectorLink(const std::string &link_name)
01301 {
01302 if (impl_->getEndEffectorLink().empty() || link_name.empty())
01303 return false;
01304 impl_->setEndEffectorLink(link_name);
01305 impl_->setTargetType(POSE);
01306 return true;
01307 }
01308
01309 bool moveit::planning_interface::MoveGroup::setEndEffector(const std::string &eef_name)
01310 {
01311 const robot_model::JointModelGroup *jmg = impl_->getRobotModel()->getEndEffector(eef_name);
01312 if (jmg)
01313 return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
01314 return false;
01315 }
01316
01317 void moveit::planning_interface::MoveGroup::clearPoseTarget(const std::string &end_effector_link)
01318 {
01319 impl_->clearPoseTarget(end_effector_link);
01320 }
01321
01322 void moveit::planning_interface::MoveGroup::clearPoseTargets()
01323 {
01324 impl_->clearPoseTargets();
01325 }
01326
01327 bool moveit::planning_interface::MoveGroup::setPoseTarget(const Eigen::Affine3d &pose, const std::string &end_effector_link)
01328 {
01329 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01330 tf::poseEigenToMsg(pose, pose_msg[0].pose);
01331 pose_msg[0].header.frame_id = getPoseReferenceFrame();
01332 pose_msg[0].header.stamp = ros::Time::now();
01333 return setPoseTargets(pose_msg, end_effector_link);
01334 }
01335
01336 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::Pose &target, const std::string &end_effector_link)
01337 {
01338 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01339 pose_msg[0].pose = target;
01340 pose_msg[0].header.frame_id = getPoseReferenceFrame();
01341 pose_msg[0].header.stamp = ros::Time::now();
01342 return setPoseTargets(pose_msg, end_effector_link);
01343 }
01344
01345 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::PoseStamped &target, const std::string &end_effector_link)
01346 {
01347 std::vector<geometry_msgs::PoseStamped> targets(1, target);
01348 return setPoseTargets(targets, end_effector_link);
01349 }
01350
01351 bool moveit::planning_interface::MoveGroup::setPoseTargets(const EigenSTL::vector_Affine3d &target, const std::string &end_effector_link)
01352 {
01353 std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
01354 ros::Time tm = ros::Time::now();
01355 const std::string &frame_id = getPoseReferenceFrame();
01356 for (std::size_t i = 0 ; i < target.size() ; ++i)
01357 {
01358 tf::poseEigenToMsg(target[i], pose_out[i].pose);
01359 pose_out[i].header.stamp = tm;
01360 pose_out[i].header.frame_id = frame_id;
01361 }
01362 return setPoseTargets(pose_out, end_effector_link);
01363 }
01364
01365 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::Pose> &target, const std::string &end_effector_link)
01366 {
01367 std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
01368 ros::Time tm = ros::Time::now();
01369 const std::string &frame_id = getPoseReferenceFrame();
01370 for (std::size_t i = 0 ; i < target.size() ; ++i)
01371 {
01372 target_stamped[i].pose = target[i];
01373 target_stamped[i].header.stamp = tm;
01374 target_stamped[i].header.frame_id = frame_id;
01375 }
01376 return setPoseTargets(target_stamped, end_effector_link);
01377 }
01378
01379 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &target, const std::string &end_effector_link)
01380 {
01381 if (target.empty())
01382 {
01383 ROS_ERROR("No pose specified as goal target");
01384 return false;
01385 }
01386 else
01387 {
01388 impl_->setTargetType(POSE);
01389 return impl_->setPoseTargets(target, end_effector_link);
01390 }
01391 }
01392
01393 const geometry_msgs::PoseStamped& moveit::planning_interface::MoveGroup::getPoseTarget(const std::string &end_effector_link) const
01394 {
01395 return impl_->getPoseTarget(end_effector_link);
01396 }
01397
01398 const std::vector<geometry_msgs::PoseStamped>& moveit::planning_interface::MoveGroup::getPoseTargets(const std::string &end_effector_link) const
01399 {
01400 return impl_->getPoseTargets(end_effector_link);
01401 }
01402
01403 namespace
01404 {
01405 inline void transformPose(const tf::Transformer& tf, const std::string &desired_frame, geometry_msgs::PoseStamped &target)
01406 {
01407 if (desired_frame != target.header.frame_id)
01408 {
01409 tf::Pose pose;
01410 tf::poseMsgToTF(target.pose, pose);
01411 tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
01412 tf::Stamped<tf::Pose> stamped_target_out;
01413 tf.transformPose(desired_frame, stamped_target, stamped_target_out);
01414 target.header.frame_id = stamped_target_out.frame_id_;
01415
01416 tf::poseTFToMsg(stamped_target_out, target.pose);
01417 }
01418 }
01419 }
01420
01421 bool moveit::planning_interface::MoveGroup::setPositionTarget(double x, double y, double z, const std::string &end_effector_link)
01422 {
01423 geometry_msgs::PoseStamped target;
01424 if (impl_->hasPoseTarget(end_effector_link))
01425 {
01426 target = getPoseTarget(end_effector_link);
01427 transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01428 }
01429 else
01430 {
01431 target.pose.orientation.x = 0.0;
01432 target.pose.orientation.y = 0.0;
01433 target.pose.orientation.z = 0.0;
01434 target.pose.orientation.w = 1.0;
01435 target.header.frame_id = impl_->getPoseReferenceFrame();
01436 }
01437
01438 target.pose.position.x = x;
01439 target.pose.position.y = y;
01440 target.pose.position.z = z;
01441 bool result = setPoseTarget(target, end_effector_link);
01442 impl_->setTargetType(POSITION);
01443 return result;
01444 }
01445
01446 bool moveit::planning_interface::MoveGroup::setRPYTarget(double r, double p, double y, const std::string &end_effector_link)
01447 {
01448 geometry_msgs::PoseStamped target;
01449 if (impl_->hasPoseTarget(end_effector_link))
01450 {
01451 target = getPoseTarget(end_effector_link);
01452 transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01453 }
01454 else
01455 {
01456 target.pose.position.x = 0.0;
01457 target.pose.position.y = 0.0;
01458 target.pose.position.z = 0.0;
01459 target.header.frame_id = impl_->getPoseReferenceFrame();
01460 }
01461
01462 tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
01463 bool result = setPoseTarget(target, end_effector_link);
01464 impl_->setTargetType(ORIENTATION);
01465 return result;
01466 }
01467
01468 bool moveit::planning_interface::MoveGroup::setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link)
01469 {
01470 geometry_msgs::PoseStamped target;
01471 if (impl_->hasPoseTarget(end_effector_link))
01472 {
01473 target = getPoseTarget(end_effector_link);
01474 transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01475 }
01476 else
01477 {
01478 target.pose.position.x = 0.0;
01479 target.pose.position.y = 0.0;
01480 target.pose.position.z = 0.0;
01481 target.header.frame_id = impl_->getPoseReferenceFrame();
01482 }
01483
01484 target.pose.orientation.x = x;
01485 target.pose.orientation.y = y;
01486 target.pose.orientation.z = z;
01487 target.pose.orientation.w = w;
01488 bool result = setPoseTarget(target, end_effector_link);
01489 impl_->setTargetType(ORIENTATION);
01490 return result;
01491 }
01492
01493 void moveit::planning_interface::MoveGroup::setPoseReferenceFrame(const std::string &pose_reference_frame)
01494 {
01495 impl_->setPoseReferenceFrame(pose_reference_frame);
01496 }
01497
01498 const std::string& moveit::planning_interface::MoveGroup::getPoseReferenceFrame() const
01499 {
01500 return impl_->getPoseReferenceFrame();
01501 }
01502
01503 double moveit::planning_interface::MoveGroup::getGoalJointTolerance() const
01504 {
01505 return impl_->getGoalJointTolerance();
01506 }
01507
01508 double moveit::planning_interface::MoveGroup::getGoalPositionTolerance() const
01509 {
01510 return impl_->getGoalPositionTolerance();
01511 }
01512
01513 double moveit::planning_interface::MoveGroup::getGoalOrientationTolerance() const
01514 {
01515 return impl_->getGoalOrientationTolerance();
01516 }
01517
01518 void moveit::planning_interface::MoveGroup::setGoalTolerance(double tolerance)
01519 {
01520 setGoalJointTolerance(tolerance);
01521 setGoalPositionTolerance(tolerance);
01522 setGoalOrientationTolerance(tolerance);
01523 }
01524
01525 void moveit::planning_interface::MoveGroup::setGoalJointTolerance(double tolerance)
01526 {
01527 impl_->setGoalJointTolerance(tolerance);
01528 }
01529
01530 void moveit::planning_interface::MoveGroup::setGoalPositionTolerance(double tolerance)
01531 {
01532 impl_->setGoalPositionTolerance(tolerance);
01533 }
01534
01535 void moveit::planning_interface::MoveGroup::setGoalOrientationTolerance(double tolerance)
01536 {
01537 impl_->setGoalOrientationTolerance(tolerance);
01538 }
01539
01540 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string &name)
01541 {
01542 rememberJointValues(name, getCurrentJointValues());
01543 }
01544
01545 bool moveit::planning_interface::MoveGroup::startStateMonitor(double wait)
01546 {
01547 return impl_->startStateMonitor(wait);
01548 }
01549
01550 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentJointValues()
01551 {
01552 robot_state::RobotStatePtr current_state;
01553 std::vector<double> values;
01554 if (impl_->getCurrentState(current_state))
01555 current_state->copyJointGroupPositions(getName(), values);
01556 return values;
01557 }
01558
01559 std::vector<double> moveit::planning_interface::MoveGroup::getRandomJointValues()
01560 {
01561 std::vector<double> r;
01562 impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
01563 return r;
01564 }
01565
01566 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getRandomPose(const std::string &end_effector_link)
01567 {
01568 const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01569 Eigen::Affine3d pose;
01570 pose.setIdentity();
01571 if (eef.empty())
01572 ROS_ERROR("No end-effector specified");
01573 else
01574 {
01575 robot_state::RobotStatePtr current_state;
01576 if (impl_->getCurrentState(current_state))
01577 {
01578 current_state->setToRandomPositions(impl_->getJointModelGroup());
01579 const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01580 if (lm)
01581 pose = current_state->getGlobalLinkTransform(lm);
01582 }
01583 }
01584 geometry_msgs::PoseStamped pose_msg;
01585 pose_msg.header.stamp = ros::Time::now();
01586 pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01587 tf::poseEigenToMsg(pose, pose_msg.pose);
01588 return pose_msg;
01589 }
01590
01591 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getCurrentPose(const std::string &end_effector_link)
01592 {
01593 const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01594 Eigen::Affine3d pose;
01595 pose.setIdentity();
01596 if (eef.empty())
01597 ROS_ERROR("No end-effector specified");
01598 else
01599 {
01600 robot_state::RobotStatePtr current_state;
01601 if (impl_->getCurrentState(current_state))
01602 {
01603 const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01604 if (lm)
01605 pose = current_state->getGlobalLinkTransform(lm);
01606 }
01607 }
01608 geometry_msgs::PoseStamped pose_msg;
01609 pose_msg.header.stamp = ros::Time::now();
01610 pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01611 tf::poseEigenToMsg(pose, pose_msg.pose);
01612 return pose_msg;
01613 }
01614
01615 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentRPY(const std::string &end_effector_link)
01616 {
01617 std::vector<double> result;
01618 const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01619 if (eef.empty())
01620 ROS_ERROR("No end-effector specified");
01621 else
01622 {
01623 robot_state::RobotStatePtr current_state;
01624 if (impl_->getCurrentState(current_state))
01625 {
01626 const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01627 if (lm)
01628 {
01629 result.resize(3);
01630 tf::Matrix3x3 ptf;
01631 tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
01632 tfScalar pitch, roll, yaw;
01633 ptf.getRPY(roll, pitch, yaw);
01634 result[0] = roll;
01635 result[1] = pitch;
01636 result[2] = yaw;
01637 }
01638 }
01639 }
01640 return result;
01641 }
01642
01643 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getActiveJoints() const
01644 {
01645 return impl_->getJointModelGroup()->getActiveJointModelNames();
01646 }
01647
01648 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJoints() const
01649 {
01650 return impl_->getJointModelGroup()->getJointModelNames();
01651 }
01652
01653 unsigned int moveit::planning_interface::MoveGroup::getVariableCount() const
01654 {
01655 return impl_->getJointModelGroup()->getVariableCount();
01656 }
01657
01658 robot_state::RobotStatePtr moveit::planning_interface::MoveGroup::getCurrentState()
01659 {
01660 robot_state::RobotStatePtr current_state;
01661 impl_->getCurrentState(current_state);
01662 return current_state;
01663 }
01664
01665 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string &name, const std::vector<double> &values)
01666 {
01667 remembered_joint_values_[name] = values;
01668 }
01669
01670 void moveit::planning_interface::MoveGroup::forgetJointValues(const std::string &name)
01671 {
01672 remembered_joint_values_.erase(name);
01673 }
01674
01675 void moveit::planning_interface::MoveGroup::allowLooking(bool flag)
01676 {
01677 impl_->allowLooking(flag);
01678 }
01679
01680 void moveit::planning_interface::MoveGroup::allowReplanning(bool flag)
01681 {
01682 impl_->allowReplanning(flag);
01683 }
01684
01685 std::vector<std::string> moveit::planning_interface::MoveGroup::getKnownConstraints() const
01686 {
01687 return impl_->getKnownConstraints();
01688 }
01689
01690 moveit_msgs::Constraints moveit::planning_interface::MoveGroup::getPathConstraints() const
01691 {
01692 return impl_->getPathConstraints();
01693 }
01694
01695 bool moveit::planning_interface::MoveGroup::setPathConstraints(const std::string &constraint)
01696 {
01697 return impl_->setPathConstraints(constraint);
01698 }
01699
01700 void moveit::planning_interface::MoveGroup::setPathConstraints(const moveit_msgs::Constraints &constraint)
01701 {
01702 impl_->setPathConstraints(constraint);
01703 }
01704
01705 void moveit::planning_interface::MoveGroup::clearPathConstraints()
01706 {
01707 impl_->clearPathConstraints();
01708 }
01709
01710 void moveit::planning_interface::MoveGroup::setConstraintsDatabase(const std::string &host, unsigned int port)
01711 {
01712 impl_->initializeConstraintsStorage(host, port);
01713 }
01714
01715 void moveit::planning_interface::MoveGroup::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
01716 {
01717 impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
01718 }
01719
01720 void moveit::planning_interface::MoveGroup::setPlanningTime(double seconds)
01721 {
01722 impl_->setPlanningTime(seconds);
01723 }
01724
01725 double moveit::planning_interface::MoveGroup::getPlanningTime() const
01726 {
01727 return impl_->getPlanningTime();
01728 }
01729
01730 void moveit::planning_interface::MoveGroup::setSupportSurfaceName(const std::string &name)
01731 {
01732 impl_->setSupportSurfaceName(name);
01733 }
01734
01735 const std::string& moveit::planning_interface::MoveGroup::getPlanningFrame() const
01736 {
01737 return impl_->getRobotModel()->getModelFrame();
01738 }
01739
01740 bool moveit::planning_interface::MoveGroup::attachObject(const std::string &object, const std::string &link)
01741 {
01742 return attachObject(object, link, std::vector<std::string>());
01743 }
01744
01745 bool moveit::planning_interface::MoveGroup::attachObject(const std::string &object, const std::string &link, const std::vector<std::string> &touch_links)
01746 {
01747 return impl_->attachObject(object, link, touch_links);
01748 }
01749
01750 bool moveit::planning_interface::MoveGroup::detachObject(const std::string &name)
01751 {
01752 return impl_->detachObject(name);
01753 }