bool idynChain2kdlChain(iCub::iDyn::iDynChain & idynChain,KDL::Chain & kdlChain,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) { int n_links, i; bool use_names; n_links = idynChain.getN(); //In iDyn, the joints are only 1 DOF (not 0, not more) int n_joints = idynChain.getN(); if(n_links <= 0 ) return false; if( (int)link_names.size() < n_links || (int)joint_names.size() < n_joints ) { use_names = false; } else { use_names = true; } KDL::Frame kdlFrame = KDL::Frame(); KDL::Frame kdl_H = KDL::Frame(); KDL::Joint kdlJoint = KDL::Joint(); kdlChain = KDL::Chain(); KDL::Segment kdlSegment = KDL::Segment(); KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia(); 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); } for(i=0; i<n_links && i < max_links; i++) { //forgive him, as he does not know what is doing iCub::iKin::iKinLink & link_current = idynChain[i]; //For the first link and the last link, take in account also H0 and HN if ( i == n_links - 1) { if( final_frame_name.length() == 0 ) { 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()); } } 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); if( !ret ) return false; KDL::Joint jnt_idyn; //\todo: joint can also be blocked at values different from 0.0 if( idynChain.isLinkBlocked(i) ) { if( use_names ) { kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::None),kdlFrame,kdlRigidBodyInertia); } else { kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame,kdlRigidBodyInertia); } } else { if( use_names ) { kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia); } else { kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia); } } kdlChain.addSegment(kdlSegment); } //if specified, add a final fake link 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); } //Considering the H0 transformation if( initial_frame_name.length() == 0 ) { KDL::Chain new_chain; KDL::Frame kdl_H0; idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0); //std::cout << "KDL_h0 " << kdl_H0 << std::endl; addBaseTransformation(kdlChain,new_chain,kdl_H0); kdlChain = new_chain; } return true; }
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; }