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


ias_arm_kinematics
Author(s): David Lu - Alexis Maldonado
autogenerated on Thu May 23 2013 11:33:06