yarp::sig::Vector idynSensorGetSensorMoment_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
    
    //H_i^s
    yarp::sig::Matrix H = iCub::ctrl::SE3inv(idynSensor.getH());
    int sensor_link = idynSensor.getSensorLink();
    
    KDL::Wrenches f_i;

    int nj = idynChain.getN();
    int ns = nj+1;
    //int sensor_link = idynSensor.getSensorLink();


    yarp::sig::Vector f(3),mu(3);
    
    f_i =  idynChainGet_usingKDL_aux(idynChain,idynSensor,ddp0);

    assert(f_i.size() == ns);
    

    to_iDyn(f_i[sensor_link+1].force,f);
    f = H.submatrix(0,2,0,2)*f;
    
    to_iDyn(f_i[sensor_link+1].torque,mu);
    yarp::sig::Vector r_n = H.subcol(0,3,3);
    mu = H.submatrix(0,2,0,2)*mu+yarp::math::cross(r_n,f);
    return mu;
}
yarp::sig::Vector idynSensorGetSensorForce_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
    //H_i^s
    yarp::sig::Matrix H = localSE3inv(idynSensor.getH());
    int sensor_link = idynSensor.getSensorLink();
    
    KDL::Wrenches f_i;

    int nj = idynChain.getN();
    int ns = nj+1;
    //int sensor_link = idynSensor.getSensorLink();


    yarp::sig::Vector f(3);
    
    f_i =  idynChainGet_usingKDL_aux(idynChain,idynSensor,ddp0);

    assert(f_i.size() == ns);
    

    to_iDyn(f_i[sensor_link+1].force,f);
    f = H.submatrix(0,2,0,2)*f;
    return f;
}
yarp::sig::Matrix idynChainGetForces_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
    KDL::Wrenches f_i;

    int nj = idynChain.getN();
    int ns = nj+1;
    int sensor_link = idynSensor.getSensorLink();


    yarp::sig::Vector f(3);
    yarp::sig::Matrix F(3,nj);
    
    F.zero();
    
    f_i =  idynChainGet_usingKDL_aux(idynChain,idynSensor,ddp0);

    assert(f_i.size() == ns);
    
    int i,j;
    for(i=0,j=0; i < ns; i++) {
        if( i < sensor_link ) {
            assert(i != ns -1);
            //DEBUG
            //cout << "converting old, f_i " << i+1 << endl;
            to_iDyn(f_i[i+1].force,f);
            f = ((idynChain)[i+1]).getR()*f;
        }
    
        if( i == sensor_link-1 )
        {
            assert(i != ns -1);
            //pay attention to this, for KDL the wrench is referred to the sensor reference frame
            iCub::iKin::iKinLink link_sensor = idynChain[sensor_link];
            /*
            double angSensorLink = link_sensor.getAng();
            yarp::sig::Matrix  H_sensor_link = (link_sensor.getH(0.0));
            link_sensor.setAng(angSensorLink);
            */
             yarp::sig::Matrix  H_sensor_link = (link_sensor.getH());
            //H_0 = H_{i}^{i-1}*H_{s}^{i}
            yarp::sig::Matrix H_0 = H_sensor_link  * (idynSensor.getH());
            
            to_iDyn(f_i[i+1].force,f);
            f = H_0.submatrix(0,2,0,2)*f;
        }
        if( i > sensor_link ) 
        {
            if( i != ns-1 ) {
                //DEBUG
                //cout << "converting new, f_i " << i+1 << endl;
                to_iDyn(f_i[i+1].force,f);
                //printVector("f",f);
                f = ((idynChain)[i]).getR()*f;
                //printVector("f",f);

            } else {
                f.zero();
            }
            
        }
        if(i!=sensor_link) {
            //cout << "j" << j << endl;
            F.setCol(j,f);
            j++;
        }
    }
    return F;
}
yarp::sig::Matrix idynChainGetMoments_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
    KDL::Wrenches f_i;
    int nj = idynChain.getN();
    
    int ns = nj+1;
    int sensor_link = idynSensor.getSensorLink();

    yarp::sig::Vector mu(3),f(3);
    yarp::sig::Matrix Mu(3,nj);
    
    Mu.zero();

    f_i =  idynChainGet_usingKDL_aux(idynChain,idynSensor,ddp0);
    
    //Debug
    /*
    std::cout << "f_i with sensor " << std::endl;
    for(int p=0;p<f_i.size();p++)
    {
        std::cout << f_i[p] << std::endl;
    }
    */
    
    int i,j;
    for(i=0,j=0; i < ns; i++) {
        mu.zero();
        if( i < sensor_link-1 ) {
            assert(i != ns -1);
            to_iDyn(f_i[i+1].force,f);
            f = ((idynChain)[i+1]).getR()*f;
            to_iDyn(f_i[i+1].torque,mu);
            yarp::sig::Vector r_n = ((idynChain)[i+1]).getH().subcol(0,3,3);
            mu = ((idynChain)[i+1]).getR()*mu+cross(r_n,f);
        }
        if( i == sensor_link-1 )
        {
            assert(i != ns -1);
            //pay attention to this, for KDL the wrench is referred to the sensor reference frame
            iCub::iKin::iKinLink link_sensor = idynChain[sensor_link];
            //double angSensorLink = link_sensor.getAng();
            yarp::sig::Matrix  H_sensor_link = (link_sensor.getH());
            //link_sensor.setAng(angSensorLink);
            yarp::sig::Matrix H_0 = H_sensor_link  * (idynSensor.getH());
            
            to_iDyn(f_i[i+1].force,f);
            f = H_0.submatrix(0,2,0,2)*f;
            to_iDyn(f_i[i+1].torque,mu);
            yarp::sig::Vector r_n = H_0.subcol(0,3,3);
            mu =  H_0.submatrix(0,2,0,2)*mu+cross(r_n,f);
        }
        if( i > sensor_link-1 ) 
        {
            if( i != ns-1 ) {
                to_iDyn(f_i[i+1].force,f);
                f = ((idynChain)[i]).getR()*f;
                to_iDyn(f_i[i+1].torque,mu);
                yarp::sig::Vector r_n = ((idynChain)[i]).getH().subcol(0,3,3);
                mu = ((idynChain)[i]).getR()*mu+yarp::math::cross(r_n,f);
            } else {
                mu.zero();
            }
            
        }
        if(i!=sensor_link) {
            Mu.setCol(j,mu);
            j++;
        }
    }
    
    /*for(int i=0; i < nj; i++ ) {
      if( i != nj -1 ) {  
        to_iDyn(f_i[i+1].force,f);
        f = (idynChain[i+1]).getR()*f;
        to_iDyn(f_i[i+1].torque,mu);
        Vector r_n = ((idynChain)[i+1]).getH().subcol(0,3,3);
        mu = ((idynChain)[i+1]).getR()*mu+cross(r_n,f);
      } else {
        mu.zero();
      }*/
    return Mu;
}
Example #5
0
bool idynSensorChain2kdlChain(iCub::iDyn::iDynChain & idynChain,
                              iCub::iDyn::iDynInvSensor & idynSensor,
                              KDL::Chain & kdlChain,
                              KDL::Frame & H_sensor_dh_child,
                              std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
    bool use_names;
    int n_links, i, sensor_link;
    n_links = idynChain.getN();
    sensor_link = idynSensor.getSensorLink();

    int kdl_links = n_links + 1; //The sensor links transform a link in two different links
    int kdl_joints = kdl_links;

    if(n_links <= 0 ) return false;

    if( (int)link_names.size() < kdl_links || (int)joint_names.size() < kdl_joints ) {
        use_names = false;
    } else {
        use_names = true;
    }


    KDL::Frame kdlFrame = KDL::Frame();
    KDL::Frame kdl_H;
    kdlChain = KDL::Chain();
    KDL::Segment kdlSegment = KDL::Segment();
    KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();

    int kdl_i = 0;

    if( initial_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
        kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }


    KDL::Frame remainder_frame = KDL::Frame::Identity();
    for(i=0; i<n_links; i++)
    {
        if( i != sensor_link ) {
            //forgive him, as he does not know what is doing
            iCub::iKin::iKinLink & link_current = idynChain[i];
            //For the last link, take in account also HN
            if ( i == n_links - 1) {
                idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
            } else {
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
            }

            bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
            assert(ret);
            if(!ret) return false;

            //For the joint after the ft sensor, consider the offset between the ft sensor
            // and the joint

            if( idynChain.isLinkBlocked(i) ) {
                // if it is blocked, it is easy to add the remainder frame
                if( use_names ) {
                    kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                    kdl_i++;
                } else {
                    kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                }
            } else {
                KDL::Vector new_joint_axis, new_joint_origin;
                new_joint_axis = remainder_frame.M*KDL::Vector(0.0,0.0,0.1);
                new_joint_origin = remainder_frame.p;
                if( use_names ) {
                    kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                    kdl_i++;
                } else {
                    kdlSegment = KDL::Segment(KDL::Joint(new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                }
            }
            kdlChain.addSegment(kdlSegment);
            remainder_frame = KDL::Frame::Identity();
        } else {
            //( i == segment_link )
            double m,m0,m1;
            yarp::sig::Vector r0(3),r1(3),r(3),rgg0(3),rgg1(3),r_i_s_wrt_i(3),r_i_C0_wrt_i;
            yarp::sig::Matrix I,I0,I1;
            yarp::sig::Matrix R_s_wrt_i;

            iCub::iKin::iKinLink & link_current = idynChain[sensor_link];
            KDL::Frame kdlFrame_0 = KDL::Frame();
            KDL::Frame kdlFrame_1 = KDL::Frame();

            //Imagine that we have i, s , i+1

            //yarp::sig::Matrix H_0;
            //The angle of the sensor link joint is put to 0 and then restored
            double angSensorLink = link_current.getAng();
            yarp::sig::Matrix  H_sensor_link = (link_current.getH(0.0)); //H_{i-1}_i
            link_current.setAng(angSensorLink);
            //idynSensor.getH() <--> H_i_s
            yarp::sig::Matrix H_0 = H_sensor_link  * (idynSensor.getH()); // H_{i-1}_s = H_{i-1}_i*H_i_s ?
            yarp::sig::Matrix H_1 = localSE3inv(idynSensor.getH()); //H_s_{i}
            //std::cout << "H_0" << std::endl << H_0.toString() << std::endl;
            //std::cout << "H_1" << std::endl << H_1.toString() << std::endl;
            idynMatrix2kdlFrame(H_0,kdlFrame_0);
            idynMatrix2kdlFrame(H_1,kdlFrame_1);

            remainder_frame = kdlFrame_1;

            H_sensor_dh_child = remainder_frame;

            kdlFrame_1 = KDL::Frame::Identity();

            //cout << "kdlFrame_0: " << endl;
            //cout << kdlFrame_0;
            //cout << "kdlFrame_1: " << endl;
            //cout << kdlFrame_1;


            KDL::RigidBodyInertia kdlRigidBodyInertia_0 = KDL::RigidBodyInertia();
            KDL::RigidBodyInertia kdlRigidBodyInertia_1 = KDL::RigidBodyInertia();

            m = idynChain.getMass(sensor_link);
            m1 = idynSensor.getMass();
            m0 = m-m1;
            //It is not possible that the semilink is more heavy than the link!!!
            assert(m0 > 0);

            //r_{i,C_i}^i
            r = idynChain.getCOM(i).subcol(0,3,3);

            //R_s^i
            R_s_wrt_i = idynSensor.getH().submatrix(0,2,0,2);

            r_i_s_wrt_i = idynSensor.getH().subcol(0,3,3);

            //r0 := r_{s,C_{{vl}_0}}^s
            //r1 := r_{i,C_{{vl}_1}}^i



            //
            r1 = r_i_s_wrt_i +  R_s_wrt_i*(idynSensor.getCOM().subcol(0,3,3));

            //cout << "m0: " << m0 << endl;
            r_i_C0_wrt_i = (1/m0)*(m*r-m1*r1);

            r0 = R_s_wrt_i.transposed()*(r_i_C0_wrt_i-r_i_s_wrt_i);

            I = idynChain.getInertia(i);

            I1 = R_s_wrt_i*idynSensor.getInertia()*R_s_wrt_i.transposed();
            rgg0 = -1*r+r_i_C0_wrt_i;
            rgg1 = -1*r+r1;

            I0 = R_s_wrt_i.transposed()*(I - I1 + m1*crossProductMatrix(rgg1)*crossProductMatrix(rgg1) +  m0*crossProductMatrix(rgg0)*crossProductMatrix(rgg0))*R_s_wrt_i;

            //DEBUG
            //printMatrix("I",I);
            //printMatrix("I1",I1);
            //printMatrix("I0",I0);
            //printMatrix("cross",crossProductMatrix(rgg1));

            //cout << "m0: " << m0 << endl;
            //printVector("r0",r0);
            //printMatrix("I0",I0);
            bool ret;
            ret = idynDynamicalParameters2kdlRigidBodyInertia(m0,r0,I0,kdlRigidBodyInertia_0);
            assert(ret);
            if(!ret) return false;
            ret = idynDynamicalParameters2kdlRigidBodyInertia(m1,r1,I1,kdlRigidBodyInertia_1);
            assert(ret);
            if(!ret) return false;

            KDL::RigidBodyInertia kdlRigidBodyInertia_1_buf;
            kdlRigidBodyInertia_1_buf = remainder_frame*kdlRigidBodyInertia_1;
            kdlRigidBodyInertia_1 = kdlRigidBodyInertia_1_buf;

            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
                kdl_i++;
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
            }
            kdlChain.addSegment(kdlSegment);

            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
                kdl_i++;
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
            }
            kdlChain.addSegment(kdlSegment);
        }

    }
    //Final link can be segment
    if( final_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
        kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }

    if( max_links < (int)kdlChain.getNrOfSegments() )
    {
        KDL::Chain new_kdlChain;
        for(int p=0; p < max_links; p++)
        {
            new_kdlChain.addSegment(kdlChain.getSegment(p));
        }
        kdlChain = new_kdlChain;
    }

    //Considering the H0 transformation
    if( initial_frame_name.length() == 0 ) {
        KDL::Chain new_chain;
        KDL::Frame kdl_H0;
        idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
        addBaseTransformation(kdlChain,new_chain,kdl_H0);
        kdlChain = new_chain;
    }

    return true;
}