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 #include "kinematics_service_capability.h"
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <moveit/move_group/capability_names.h>
00042
00043 move_group::MoveGroupKinematicsService::MoveGroupKinematicsService():
00044 MoveGroupCapability("KinematicsService")
00045 {
00046 }
00047
00048 void move_group::MoveGroupKinematicsService::initialize()
00049 {
00050 fk_service_ = root_node_handle_.advertiseService(FK_SERVICE_NAME, &MoveGroupKinematicsService::computeFKService, this);
00051 ik_service_ = root_node_handle_.advertiseService(IK_SERVICE_NAME, &MoveGroupKinematicsService::computeIKService, this);
00052 }
00053
00054 namespace
00055 {
00056 bool isIKSolutionValid(const planning_scene::PlanningScene *planning_scene,
00057 const kinematic_constraints::KinematicConstraintSet *constraint_set,
00058 robot_state::RobotState *state,
00059 const robot_model::JointModelGroup *jmg,
00060 const double *ik_solution)
00061 {
00062 state->setJointGroupPositions(jmg, ik_solution);
00063 state->update();
00064 return (!planning_scene || !planning_scene->isStateColliding(*state, jmg->getName())) &&
00065 (!constraint_set || constraint_set->decide(*state).satisfied);
00066 }
00067 }
00068
00069 void move_group::MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest &req,
00070 moveit_msgs::RobotState &solution,
00071 moveit_msgs::MoveItErrorCodes &error_code,
00072 robot_state::RobotState &rs,
00073 const robot_state::GroupStateValidityCallbackFn &constraint) const
00074 {
00075 const robot_state::JointModelGroup *jmg = rs.getJointModelGroup(req.group_name);
00076 if (jmg)
00077 {
00078 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00079 const std::string &default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00080
00081 if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1)
00082 {
00083 geometry_msgs::PoseStamped req_pose = req.pose_stamped_vector.empty() ? req.pose_stamped : req.pose_stamped_vector[0];
00084 std::string ik_link = req.pose_stamped_vector.empty() ? (req.ik_link_names.empty() ? "" : req.ik_link_names[0]) : req.ik_link_name;
00085
00086 if (performTransform(req_pose, default_frame))
00087 {
00088 bool result_ik = false;
00089 if (ik_link.empty())
00090 result_ik = rs.setFromIK(jmg, req_pose.pose, req.attempts, req.timeout.toSec(), constraint);
00091 else
00092 result_ik = rs.setFromIK(jmg, req_pose.pose, ik_link, req.attempts, req.timeout.toSec(), constraint);
00093
00094 if(result_ik)
00095 {
00096 robot_state::robotStateToRobotStateMsg(rs, solution, false);
00097 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00098 }
00099 else
00100 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00101 }
00102 else
00103 error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00104 }
00105 else
00106 {
00107 if (req.pose_stamped_vector.size() != req.ik_link_names.size())
00108 error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00109 else
00110 {
00111 bool ok = true;
00112 EigenSTL::vector_Affine3d req_poses(req.pose_stamped_vector.size());
00113 for (std::size_t k = 0 ; k < req.pose_stamped_vector.size() ; ++k)
00114 {
00115 geometry_msgs::PoseStamped msg = req.pose_stamped_vector[k];
00116 if (performTransform(msg, default_frame))
00117 tf::poseMsgToEigen(msg.pose, req_poses[k]);
00118 else
00119 {
00120 error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00121 ok = false;
00122 break;
00123 }
00124 }
00125 if (ok)
00126 {
00127 if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.attempts, req.timeout.toSec(), constraint))
00128 {
00129 robot_state::robotStateToRobotStateMsg(rs, solution, false);
00130 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00131 }
00132 else
00133 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00134 }
00135 }
00136 }
00137 }
00138 else
00139 error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00140 }
00141
00142 bool move_group::MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Request &req, moveit_msgs::GetPositionIK::Response &res)
00143 {
00144 context_->planning_scene_monitor_->updateFrameTransforms();
00145
00146
00147 if (req.ik_request.avoid_collisions || !kinematic_constraints::isEmpty(req.ik_request.constraints))
00148 {
00149 planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
00150 kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
00151 robot_state::RobotState rs = ls->getCurrentState();
00152 kset.add(req.ik_request.constraints, ls->getTransforms());
00153 computeIK(req.ik_request, res.solution, res.error_code, rs, boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ?
00154 static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() : NULL,
00155 kset.empty() ? NULL : &kset, _1, _2, _3));
00156 }
00157 else
00158 {
00159
00160 robot_state::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00161 computeIK(req.ik_request, res.solution, res.error_code, rs);
00162 }
00163
00164 return true;
00165 }
00166
00167 bool move_group::MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Request &req, moveit_msgs::GetPositionFK::Response &res)
00168 {
00169 if (req.fk_link_names.empty())
00170 {
00171 ROS_ERROR("No links specified for FK request");
00172 res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00173 return true;
00174 }
00175
00176 context_->planning_scene_monitor_->updateFrameTransforms();
00177
00178 const std::string &default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00179 bool do_transform = !req.header.frame_id.empty() && !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame)
00180 && context_->planning_scene_monitor_->getTFClient();
00181 bool tf_problem = false;
00182
00183 robot_state::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00184 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00185 for (std::size_t i = 0 ; i < req.fk_link_names.size() ; ++i)
00186 if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i]))
00187 {
00188 res.pose_stamped.resize(res.pose_stamped.size() + 1);
00189 tf::poseEigenToMsg(rs.getGlobalLinkTransform(req.fk_link_names[i]), res.pose_stamped.back().pose);
00190 res.pose_stamped.back().header.frame_id = default_frame;
00191 res.pose_stamped.back().header.stamp = ros::Time::now();
00192 if (do_transform)
00193 if (!performTransform(res.pose_stamped.back(), req.header.frame_id))
00194 tf_problem = true;
00195 res.fk_link_names.push_back(req.fk_link_names[i]);
00196 }
00197 if (tf_problem)
00198 res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00199 else
00200 if (res.fk_link_names.size() == req.fk_link_names.size())
00201 res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00202 else
00203 res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00204 return true;
00205 }
00206
00207 #include <class_loader/class_loader.h>
00208 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupKinematicsService, move_group::MoveGroupCapability)