00001
00002 #include <cstring>
00003 #include <ros/ros.h>
00004 #include <tf/transform_listener.h>
00005 #include <tf_conversions/tf_kdl.h>
00006 #include <tf/transform_datatypes.h>
00007 #include <kdl_parser/kdl_parser.hpp>
00008 #include <kdl/jntarray.hpp>
00009 #include <kdl/chainiksolverpos_nr_jl.hpp>
00010 #include <kdl/chainiksolvervel_pinv.hpp>
00011 #include <kdl/chainfksolverpos_recursive.hpp>
00012 #include <kinematics_msgs/GetPositionFK.h>
00013 #include <kinematics_msgs/GetPositionIK.h>
00014 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00015 #include <kinematics_msgs/KinematicSolverInfo.h>
00016 using std::string;
00017
00018 static const std::string IK_SERVICE = "get_ik";
00019 static const std::string FK_SERVICE = "get_fk";
00020 static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
00021 static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
00022
00023 class Kinematics {
00024 public:
00025 Kinematics();
00026 bool init();
00027
00028 private:
00029 ros::NodeHandle nh, nh_private;
00030 std::string root_name, tip_name;
00031 KDL::JntArray joint_min, joint_max;
00032 KDL::Chain chain;
00033 unsigned int num_joints;
00034
00035 KDL::ChainFkSolverPos_recursive* fk_solver;
00036 KDL::ChainIkSolverPos_NR_JL *ik_solver_pos;
00037 KDL::ChainIkSolverVel_pinv* ik_solver_vel;
00038
00039 ros::ServiceServer ik_service,ik_solver_info_service;
00040 ros::ServiceServer fk_service,fk_solver_info_service;
00041
00042 tf::TransformListener tf_listener;
00043
00044 kinematics_msgs::KinematicSolverInfo info;
00045
00046 bool loadModel(const std::string xml);
00047 bool readJoints(urdf::Model &robot_model);
00048 int getJointIndex(const std::string &name);
00049 int getKDLSegmentIndex(const std::string &name);
00050
00056 bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00057 kinematics_msgs::GetPositionIK::Response &response);
00058
00064 bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00065 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00066
00072 bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00073 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00074
00080 bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00081 kinematics_msgs::GetPositionFK::Response &response);
00082 };
00083
00084
00085
00086 Kinematics::Kinematics(): nh_private ("~") {
00087 }
00088
00089 bool Kinematics::init() {
00090
00091 std::string urdf_xml, full_urdf_xml;
00092 nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
00093 nh.searchParam(urdf_xml,full_urdf_xml);
00094 ROS_DEBUG("Reading xml file from parameter server");
00095 std::string result;
00096 if (!nh.getParam(full_urdf_xml, result)) {
00097 ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
00098 return false;
00099 }
00100
00101
00102 if (!nh_private.getParam("root_name", root_name)) {
00103 ROS_FATAL("GenericIK: No root name found on parameter server");
00104 return false;
00105 }
00106 if (!nh_private.getParam("tip_name", tip_name)) {
00107 ROS_FATAL("GenericIK: No tip name found on parameter server");
00108 return false;
00109 }
00110
00111
00112 if (!loadModel(result)) {
00113 ROS_FATAL("Could not load models!");
00114 return false;
00115 }
00116
00117
00118 int maxIterations;
00119 double epsilon;
00120
00121 nh_private.param("maxIterations", maxIterations, 1000);
00122 nh_private.param("epsilon", epsilon, 1e-2);
00123
00124
00125 fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
00126 ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
00127 ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max,
00128 *fk_solver, *ik_solver_vel, maxIterations, epsilon);
00129
00130 ROS_INFO("Advertising services");
00131 fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
00132 ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
00133 ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
00134 fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
00135
00136 return true;
00137 }
00138
00139 bool Kinematics::loadModel(const std::string xml) {
00140 urdf::Model robot_model;
00141 KDL::Tree tree;
00142
00143 if (!robot_model.initString(xml)) {
00144 ROS_FATAL("Could not initialize robot model");
00145 return -1;
00146 }
00147 if (!kdl_parser::treeFromString(xml, tree)) {
00148 ROS_ERROR("Could not initialize tree object");
00149 return false;
00150 }
00151 if (!tree.getChain(root_name, tip_name, chain)) {
00152 ROS_ERROR("Could not initialize chain object");
00153 return false;
00154 }
00155
00156 if (!readJoints(robot_model)) {
00157 ROS_FATAL("Could not read information about the joints");
00158 return false;
00159 }
00160
00161 return true;
00162 }
00163
00164 bool Kinematics::readJoints(urdf::Model &robot_model) {
00165 num_joints = 0;
00166
00167 boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00168 boost::shared_ptr<const urdf::Joint> joint;
00169
00170 while (link && link->name != root_name) {
00171 joint = robot_model.getJoint(link->parent_joint->name);
00172 if (!joint) {
00173 ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
00174 return false;
00175 }
00176 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00177 ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
00178 num_joints++;
00179 }
00180 link = robot_model.getLink(link->getParent()->name);
00181 }
00182
00183 joint_min.resize(num_joints);
00184 joint_max.resize(num_joints);
00185 info.joint_names.resize(num_joints);
00186 info.link_names.resize(num_joints);
00187 info.limits.resize(num_joints);
00188
00189 link = robot_model.getLink(tip_name);
00190 unsigned int i = 0;
00191 while (link && i < num_joints) {
00192 joint = robot_model.getJoint(link->parent_joint->name);
00193 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00194 ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
00195
00196 float lower, upper;
00197 int hasLimits;
00198 if ( joint->type != urdf::Joint::CONTINUOUS ) {
00199 lower = joint->limits->lower;
00200 upper = joint->limits->upper;
00201 hasLimits = 1;
00202 } else {
00203 lower = -M_PI;
00204 upper = M_PI;
00205 hasLimits = 0;
00206 }
00207 int index = num_joints - i -1;
00208
00209 joint_min.data[index] = lower;
00210 joint_max.data[index] = upper;
00211 info.joint_names[index] = joint->name;
00212 info.link_names[index] = link->name;
00213 info.limits[index].joint_name = joint->name;
00214 info.limits[index].has_position_limits = hasLimits;
00215 info.limits[index].min_position = lower;
00216 info.limits[index].max_position = upper;
00217 i++;
00218 }
00219 link = robot_model.getLink(link->getParent()->name);
00220 }
00221 return true;
00222 }
00223
00224
00225 int Kinematics::getJointIndex(const std::string &name) {
00226 for (unsigned int i=0; i < info.joint_names.size(); i++) {
00227 if (info.joint_names[i] == name)
00228 return i;
00229 }
00230 return -1;
00231 }
00232
00233 int Kinematics::getKDLSegmentIndex(const std::string &name) {
00234 int i=0;
00235 while (i < (int)chain.getNrOfSegments()) {
00236 if (chain.getSegment(i).getName() == name) {
00237 return i+1;
00238 }
00239 i++;
00240 }
00241 return -1;
00242 }
00243
00244 bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00245 kinematics_msgs::GetPositionIK::Response &response) {
00246
00247 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00248 tf::Stamped<tf::Pose> transform;
00249 tf::Stamped<tf::Pose> transform_root;
00250 tf::poseStampedMsgToTF( pose_msg_in, transform );
00251
00252
00253 KDL::JntArray jnt_pos_in;
00254 KDL::JntArray jnt_pos_out;
00255 jnt_pos_in.resize(num_joints);
00256 for (unsigned int i=0; i < num_joints; i++) {
00257 int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
00258 if (tmp_index >=0) {
00259 jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
00260 } else {
00261 ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
00262 }
00263 }
00264
00265
00266 try {
00267 tf_listener.transformPose(root_name, transform, transform_root);
00268 } catch (...) {
00269 ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
00270 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00271 return false;
00272 }
00273
00274 KDL::Frame F_dest;
00275 tf::TransformTFToKDL(transform_root, F_dest);
00276
00277 int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
00278
00279 if (ik_valid >= 0) {
00280 response.solution.joint_state.name = info.joint_names;
00281 response.solution.joint_state.position.resize(num_joints);
00282 for (unsigned int i=0; i < num_joints; i++) {
00283 response.solution.joint_state.position[i] = jnt_pos_out(i);
00284 ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
00285 }
00286 response.error_code.val = response.error_code.SUCCESS;
00287 return true;
00288 } else {
00289 ROS_DEBUG("An IK solution could not be found");
00290 response.error_code.val = response.error_code.NO_IK_SOLUTION;
00291 return true;
00292 }
00293 }
00294
00295 bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00296 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
00297 response.kinematic_solver_info = info;
00298 return true;
00299 }
00300
00301 bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00302 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
00303 response.kinematic_solver_info = info;
00304 return true;
00305 }
00306
00307 bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00308 kinematics_msgs::GetPositionFK::Response &response) {
00309 KDL::Frame p_out;
00310 KDL::JntArray jnt_pos_in;
00311 geometry_msgs::PoseStamped pose;
00312 tf::Stamped<tf::Pose> tf_pose;
00313
00314 jnt_pos_in.resize(num_joints);
00315 for (unsigned int i=0; i < num_joints; i++) {
00316 int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
00317 if (tmp_index >=0)
00318 jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
00319 }
00320
00321 response.pose_stamped.resize(request.fk_link_names.size());
00322 response.fk_link_names.resize(request.fk_link_names.size());
00323
00324 bool valid = true;
00325 for (unsigned int i=0; i < request.fk_link_names.size(); i++) {
00326 int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
00327 ROS_DEBUG("End effector index: %d",segmentIndex);
00328 ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments());
00329 if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) {
00330 tf_pose.frame_id_ = root_name;
00331 tf_pose.stamp_ = ros::Time();
00332 tf::PoseKDLToTF(p_out,tf_pose);
00333 try {
00334 tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose);
00335 } catch (...) {
00336 ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
00337 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00338 return false;
00339 }
00340 tf::poseStampedTFToMsg(tf_pose,pose);
00341 response.pose_stamped[i] = pose;
00342 response.fk_link_names[i] = request.fk_link_names[i];
00343 response.error_code.val = response.error_code.SUCCESS;
00344 } else {
00345 ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
00346 response.error_code.val = response.error_code.NO_FK_SOLUTION;
00347 valid = false;
00348 }
00349 }
00350 return true;
00351 }
00352
00353 int main(int argc, char **argv) {
00354 ros::init(argc, argv, "arm_kinematics");
00355 Kinematics k;
00356 if (k.init()<0) {
00357 ROS_ERROR("Could not initialize kinematics node");
00358 return -1;
00359 }
00360
00361 ros::spin();
00362 return 0;
00363 }
00364