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