sr_kinematics.cpp

Go to the documentation of this file.

// bsd license blah blah
// majority of this code comes from package urdf_tool/arm_kinematics
// written by David Lu!!
// the IK is our own computation the FK will be in a near future.

#include <cstring>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <tf/transform_datatypes.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kinematics_msgs/GetPositionFK.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/KinematicSolverInfo.h>
#include <urdf/model.h>
#include <string>

using std::string;

static const std::string IK_SERVICE = "get_ik";
static const std::string FK_SERVICE = "get_fk";
static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
static const std::string FK_INFO_SERVICE = "get_fk_solver_info";

#define IK_EPS  1e-5

//taken from PR2_kinematics_utils... should we link to this or ?
  bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
  {
    double discriminant = b*b-4*a*c;
    if(fabs(a) < IK_EPS)
    {
      *x1 = -c/b;
      *x2 = *x1;
      return true;
    }
    //ROS_DEBUG("Discriminant: %f\n",discriminant);
    if (discriminant >= 0)
    {      
      *x1 = (-b + sqrt(discriminant))/(2*a); 
      *x2 = (-b - sqrt(discriminant))/(2*a);
      return true;
    } 
    else if(fabs(discriminant) < IK_EPS)
    {
      *x1 = -b/(2*a);
      *x2 = -b/(2*a);
      return true;
    }
    else
    {
      *x1 = -b/(2*a);
      *x2 = -b/(2*a);
      return false;
    }
  }

class Kinematics {
    public:
        Kinematics();
        bool init();

    private:
        ros::NodeHandle nh, nh_private;
        std::string root_name, finger_base_name, tip_name;
        KDL::JntArray joint_min, joint_max;
        KDL::Chain chain;
        unsigned int num_joints;
                std::vector<urdf::Pose> link_offset;
                std::vector<std::string> link_offset_name;
                std::vector<urdf::Vector3> knuckle_axis;


        KDL::ChainFkSolverPos_recursive* fk_solver;

        double epsilon;
                double length_middle;
                double length_proximal;
                unsigned int J5_idx_offset; //LF has an additionnal joint (1 when LF, 0 otherwise)

        ros::ServiceServer ik_service,ik_solver_info_service;
        ros::ServiceServer fk_service,fk_solver_info_service;

        tf::TransformListener tf_listener;

        kinematics_msgs::KinematicSolverInfo info;

        bool loadModel(const std::string xml);
        bool readJoints(urdf::Model &robot_model);
        int getJointIndex(const std::string &name);
        int getKDLSegmentIndex(const std::string &name);

        bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
                           kinematics_msgs::GetPositionIK::Response &response);

        bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
                             kinematics_msgs::GetKinematicSolverInfo::Response &response);

        bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
                             kinematics_msgs::GetKinematicSolverInfo::Response &response);

        bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
                           kinematics_msgs::GetPositionFK::Response &response);
};



Kinematics::Kinematics(): nh_private ("~") {
}

bool Kinematics::init() {
    // Get URDF XML
    std::string urdf_xml, full_urdf_xml;
    nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
    nh.searchParam(urdf_xml,full_urdf_xml);
    ROS_DEBUG("Reading xml file from parameter server");
    std::string result;
    if (!nh.getParam(full_urdf_xml, result)) {
        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
        return false;
    }

    // Get Root and Tip From Parameter Service
    if (!nh_private.getParam("root_name", root_name)) {
        ROS_FATAL("GenericIK: No root name found on parameter server");
        return false;
    }
    if(root_name!="palm") {
                ROS_FATAL("Current solver can only resolve to root frame = palm");
                return false;
        }

    if (!nh_private.getParam("tip_name", tip_name)) {
        ROS_FATAL("GenericIK: No tip name found on parameter server");
        return false;
    }

        if(tip_name.find("distal")==string::npos) {
                ROS_FATAL("Current solver can only resolve to one of the distal frames");
                return false;
        }

        if(tip_name.find("lfdistal")!=string::npos) {
                ROS_INFO("Computing LF IK not considering J5");
                J5_idx_offset=1;
        }
        else    
                J5_idx_offset=0;


        if(tip_name.find("thdistal")!=string::npos) {
                ROS_FATAL("Current solver cannot resolve to the thdistal frame");
                return false;
        }
    
        finger_base_name=tip_name.substr(0,2);
        finger_base_name.append("knuckle");
    ROS_INFO("base_finger name %s",finger_base_name.c_str());

    // Load and Read Models
    if (!loadModel(result)) {
        ROS_FATAL("Could not load models!");
        return false;
    }

    // Get Solver Parameters
    int maxIterations;

    nh_private.param("maxIterations", maxIterations, 1000);
    nh_private.param("epsilon", epsilon, 1e-2);

    // Build Solvers
    fk_solver = new KDL::ChainFkSolverPos_recursive(chain); //keep the standard arm_kinematics fk_solver although we have ours.

    ROS_INFO("Advertising services");
    fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
    ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
    ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
    fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);

    return true;
}

bool Kinematics::loadModel(const std::string xml) {
    urdf::Model robot_model;
    KDL::Tree tree;

    if (!robot_model.initString(xml)) {
        ROS_FATAL("Could not initialize robot model");
        return -1;
    }
    if (!kdl_parser::treeFromString(xml, tree)) {
        ROS_ERROR("Could not initialize tree object");
        return false;
    }
    if (!tree.getChain(root_name, tip_name, chain)) {
        ROS_ERROR("Could not initialize chain object");
        return false;
    }

    if (!readJoints(robot_model)) {
        ROS_FATAL("Could not read information about the joints");
        return false;
    }

    return true;
}

bool Kinematics::readJoints(urdf::Model &robot_model) {
    num_joints = 0;
    // get joint maxs and mins
    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint;

        bool length_proximal_done=false;
        bool length_middle_done=false;
        urdf::Vector3 length;

    while (link && link->name != root_name) {
                //extract knuckle to palm_offset.
                if(link->name.find("knuckle")!=string::npos){
                        if(link->getParent()->name=="palm") // FF,MF,RF
                        {
                                link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
                                link_offset_name.push_back(link->name); 
                                knuckle_axis.push_back(link->parent_joint->axis);
                        }
                        else // LF
                        {       
                                //temp store the first offset
                                urdf::Pose first_offset = link->parent_joint->parent_to_joint_origin_transform;
                                std::string link_name = link->name;
                                //check for palm in parent of parent
                                if(link->getParent()->getParent()->name=="palm"){
                                        urdf::Pose second_offset = link->getParent()->parent_joint->parent_to_joint_origin_transform;
                                        urdf::Pose combined_offset(first_offset);
                                        //combine only position (so the pose between palm and lfknuckle may be wrong regarding rotations)
                                        combined_offset.position.x+=second_offset.position.x;
                                        combined_offset.position.y+=second_offset.position.y;
                                        combined_offset.position.z+=second_offset.position.z;
                                        link_offset.push_back(combined_offset);
                                        link_offset_name.push_back(link_name);
                                        knuckle_axis.push_back(link->parent_joint->axis);
                                }
                                else{ //stop at level2; structure must be false.
                                        ROS_ERROR("Could not find palm parent for this finger");
                                        return false;
                                }
                        }
                }

                // extract proximal length
                if(!length_proximal_done){
                        if(link->name.find("middle")!=string::npos){
                                length = link->parent_joint->parent_to_joint_origin_transform.position;
                                length_proximal=sqrt(length.x*length.x+length.y*length.y+length.z*length.z);
                                length_proximal_done=true;
                        }
                }

                // extract middle length
                if(!length_middle_done){
                        if(link->name.find("distal")!=string::npos){
                                length = link->parent_joint->parent_to_joint_origin_transform.position;
                                length_middle=sqrt(length.x*length.x+length.y*length.y+length.z*length.z);
                                length_middle_done=true;
                        }
                }

        joint = robot_model.getJoint(link->parent_joint->name);
        if (!joint) {
            ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
            return false;
        }
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
            num_joints++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }

    joint_min.resize(num_joints);
    joint_max.resize(num_joints);
    info.joint_names.resize(num_joints);
    info.link_names.resize(num_joints);
    info.limits.resize(num_joints);

    link = robot_model.getLink(tip_name);
    unsigned int i = 0;
    while (link && i < num_joints) {
        joint = robot_model.getJoint(link->parent_joint->name);
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );

            float lower, upper;
            int hasLimits;
            if ( joint->type != urdf::Joint::CONTINUOUS ) {
                lower = joint->limits->lower;
                upper = joint->limits->upper;
                hasLimits = 1;
            } else {
                lower = -M_PI;
                upper = M_PI;
                hasLimits = 0;
            }
            int index = num_joints - i -1;

            joint_min.data[index] = lower;
            joint_max.data[index] = upper;
            info.joint_names[index] = joint->name;
            info.link_names[index] = link->name;
            info.limits[index].joint_name = joint->name;
            info.limits[index].has_position_limits = hasLimits;
            info.limits[index].min_position = lower;
            info.limits[index].max_position = upper;
            i++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }
    return true;
}


int Kinematics::getJointIndex(const std::string &name) {
    for (unsigned int i=0; i < info.joint_names.size(); i++) {
        if (info.joint_names[i] == name)
            return i;
    }
    return -1;
}

int Kinematics::getKDLSegmentIndex(const std::string &name) {
    int i=0; 
    while (i < (int)chain.getNrOfSegments()) {
        if (chain.getSegment(i).getName() == name) {
            return i+1;
        }
        i++;
    }
    return -1;
}

bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
                               kinematics_msgs::GetPositionIK::Response &response) {

        // get the 3D position of requested goal (forget about orientation, which is not relevant)
        geometry_msgs::PointStamped point_msg_in;
        point_msg_in.header.frame_id = request.ik_request.pose_stamped.header.frame_id;
        point_msg_in.header.stamp = ros::Time::now()-ros::Duration(1);
        point_msg_in.point=request.ik_request.pose_stamped.pose.position;
        
        tf::Stamped<tf::Point> transform;
        tf::Stamped<tf::Point> transform_root;
    tf::Stamped<tf::Point> transform_finger_base;
    tf::pointStampedMsgToTF( point_msg_in, transform );

        urdf::Pose knuckle_offset;
        urdf::Vector3 J4_axis;
        float J4_dir;
        tf::Point p; //local req_point coordinates
        tf::Point pbis; //with different coordinate system
        
        // IK computation variables
        double l1=length_proximal,l2=length_middle;
        double L=0.0;
        double x1,x2;
        double thetap,thetam,l2p;
        double alpha2;
    float b,c,delta=0.01;
  
    //Do the IK
    KDL::JntArray jnt_pos_in;
    KDL::JntArray jnt_pos_out;
    jnt_pos_in.resize(num_joints);
        jnt_pos_out.resize(num_joints); 

    //Convert F to our root_frame       
        ROS_DEBUG("sr_kin: Get point in root frame");
    try {
        tf_listener.transformPoint(root_name, transform, transform_root);
    } catch (...) {
        ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
       return false;
    }
        ROS_DEBUG("root x,y,z:%f,%f,%f",transform_root.x(),transform_root.y(),transform_root.z());

        //look for knuckle corresponding to distal phalanx
        ROS_DEBUG("sr_kin: Looking for J4_axis direction and knuckle offset");
        for(unsigned int i=0;i<link_offset_name.size();i++){
                if(link_offset_name[i]==finger_base_name){
                        knuckle_offset=link_offset[i];
                        ROS_DEBUG("knuckle offset is %f,%f,%f",knuckle_offset.position.x,knuckle_offset.position.y,knuckle_offset.position.z);
                        J4_axis=knuckle_axis[i];
                        //ROS_DEBUG("J4 axis is %f,%f,%f",J4_axis.x,J4_axis.y,J4_axis.z);
                        ROS_DEBUG("J4 is %s",J4_axis.y>0?"positive":"negative");
                        break;
                }
        }

        // Change to a local computation frame (at knuckle pos but with palm orientation)
        ROS_DEBUG("sr_kin: Convert Frame to local computation frame");
        p.setValue(-knuckle_offset.position.x,-knuckle_offset.position.y,-knuckle_offset.position.z);
        p+=transform_root;
    ROS_DEBUG("x,y,z:%f,%f,%f",p.x(),p.y(),p.z());
    
        // Change frame to the one where maths have been done :-)
    pbis=p;
    pbis.setValue(p.z(),p.x(),-p.y());
        ROS_DEBUG("p2bis %f,%f,%f",pbis.x(),pbis.y(),pbis.z());
        
        // because J4 are not the same direction for each finger 
        // we need to change it according to knuckle axis direction
        ROS_DEBUG("sr_kin: Compute J4");
        J4_dir=J4_axis.y>0?1.0:-1.0;
        
        jnt_pos_out(0)=0; //to solve the case of LF;
        if(fabs(pbis.x())-epsilon>0.0){
            jnt_pos_out(0+J5_idx_offset)=J4_dir*atan( pbis.y()/pbis.x());
        }
    else{
                if(fabs(pbis.y())-epsilon>0.0){
                        float sign=pbis.y()>=0?1.0:-1.0;
                        jnt_pos_out(0+J5_idx_offset)=J4_dir*sign*M_PI_2;
                }
        else
            jnt_pos_out(0+J5_idx_offset)=0;
        }

    L=pbis.length2();
        ROS_DEBUG("L %f",L);
        ROS_DEBUG("sr_kin: Solve IK quadratic system");
        if(!solveQuadratic(l1*l2,l2*l2-3*l1*l2,-(L)+l1*l1,&x2,&x1)) //x1 x2 inversed is normal
          {
                ROS_DEBUG("No solution to quadratic eqn");
                return false;
          }
        ROS_DEBUG("x1,x2 %f,%f",x1,x2);

        // thetap et thetam are the 2 possibilities for each x1 or x2 in the isoceles triangle
        ROS_DEBUG("sr_kin: Check for acceptable solutions");
    if(x2 >0)
    {
            thetam=acos(sqrt(x2)/2);
            thetap=acos(-sqrt(x2)/2);
            l2p=2*l2* (sqrt(x2)/2);
    }
    else
    {
        if(x1>0)
        {
            thetam=acos(sqrt(x1)/2);
            thetap=acos(-sqrt(x1)/2);
            l2p=2*l2* (sqrt(x1)/2);
        }
        else
        {
            thetap=0;
            thetam=0;
            l2p=2*l2;
        }
    }

        // alpha2 is the virtual angle of the second phalanx in a system with only 2 phalanxes
        ROS_DEBUG("sr_kin: Compute virtual ALPHA");
        ROS_DEBUG("thetap,thetam, l2p %f,%f,%f",thetap,thetam,l2p);
    if(thetap >= 0 && thetap <= M_PI_2)
    {
        alpha2= 3*thetap;
    }
    else
    {
        if(thetam <= M_PI_2)
            alpha2=     3*thetam;
        else
            alpha2=0;
    }

        b=l1+l2p*cos(alpha2);
    c=l2p*sin(alpha2);

        ROS_DEBUG("sr_kin: Compute J3");
        ROS_DEBUG("alpha2,b,c %f,%f,%f",alpha2,b,c);
        if(pbis.x()<0)
        jnt_pos_out(1+J5_idx_offset)= M_PI_2+atan2(sqrt(pbis.x()*pbis.x()+pbis.y()*pbis.y()) ,pbis.z() )-atan2(c,b);
    else
        jnt_pos_out(1+J5_idx_offset)= atan2(b*pbis.z()-c*sqrt(pbis.x()*pbis.x()+pbis.y()*pbis.y()),b*sqrt(pbis.x()*pbis.x()+pbis.y()*pbis.y())+c*pbis.z());

        ROS_DEBUG("sr_kin: Compute J0=J1+J2");
        jnt_pos_out(2+J5_idx_offset)=2.0*alpha2/3.0; // sum of the angles of the last 2 phalanxes divided by 2 to get individual phalanx angle in the coupled joint
        jnt_pos_out(3+J5_idx_offset)=jnt_pos_out(2+J5_idx_offset);

    for(unsigned int i=0; i<num_joints; i++)
    {
                ROS_DEBUG("limit min-max %d, %f , %f",i,joint_min(i),joint_max(i));
                ROS_DEBUG("pos %d,%f",i,jnt_pos_out(i));
        if( jnt_pos_out(i)>joint_max(i)+delta || jnt_pos_out(i)<joint_min(i)-delta || std::isnan(jnt_pos_out(i))  )
        {
            jnt_pos_out(0+J5_idx_offset)=0;
            jnt_pos_out(1+J5_idx_offset)=0;
            jnt_pos_out(2+J5_idx_offset)=0;
                        jnt_pos_out(3+J5_idx_offset)=0;
            return 0;
        }
    }
        
    int ik_valid = 1;

    if (ik_valid >= 0) {
        response.solution.joint_state.name = info.joint_names;
        response.solution.joint_state.position.resize(num_joints);
        for (unsigned int i=0; i < num_joints; i++) {
            response.solution.joint_state.position[i] = jnt_pos_out(i);
            ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
        }
        response.error_code.val = response.error_code.SUCCESS;
        return true;
    } else {
        ROS_DEBUG("An IK solution could not be found");
        response.error_code.val = response.error_code.NO_IK_SOLUTION;
        return true;
    }
    return true;
}

bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
    response.kinematic_solver_info = info;
    return true;
}

bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
    response.kinematic_solver_info = info;
    return true;
}

bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
                               kinematics_msgs::GetPositionFK::Response &response) {
    KDL::Frame p_out;
    KDL::JntArray jnt_pos_in;
    geometry_msgs::PoseStamped pose;
    tf::Stamped<tf::Pose> tf_pose;

    jnt_pos_in.resize(num_joints);
    for (unsigned int i=0; i < num_joints; i++) {
        int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
        if (tmp_index >=0)
            jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
    }

    response.pose_stamped.resize(request.fk_link_names.size());
    response.fk_link_names.resize(request.fk_link_names.size());

    bool valid = true;
    for (unsigned int i=0; i < request.fk_link_names.size(); i++) {
        int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
        ROS_DEBUG("End effector index: %d",segmentIndex);
        ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments());
        if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) {
            tf_pose.frame_id_ = root_name;
            tf_pose.stamp_ = ros::Time();
            tf::PoseKDLToTF(p_out,tf_pose);
            try {
                tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose);
            } catch (...) {
                ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
                response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
                return false;
            }
            tf::poseStampedTFToMsg(tf_pose,pose);
            response.pose_stamped[i] = pose;
            response.fk_link_names[i] = request.fk_link_names[i];
            response.error_code.val = response.error_code.SUCCESS;
        } else {
            ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
            response.error_code.val = response.error_code.NO_FK_SOLUTION;
            valid = false;
        }
    }
    return true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "sr_kinematics");
    Kinematics k;
    if (k.init()<0) {
        ROS_ERROR("Could not initialize kinematics node");
        return -1;
    }

    ros::spin();
    return 0;
}