Exemplo n.º 1
0
      //Transformation torso right foot
      Eigen::Vector4d Tf_torso_rfoot(){
        // get transformation from right foot to torso
        NAOKinematics::kmatTable footcoord = kin.getForwardEffector((NAOKinematics::Effectors)CHAIN_R_LEG);
        Eigen::Matrix4d T_torso_rfoot;
        T_torso_rfoot(0,0) =  footcoord.getRotation()(0,0);
        T_torso_rfoot(0,1) =  footcoord.getRotation()(0,1);
        T_torso_rfoot(0,2) =  footcoord.getRotation()(0,2);
        T_torso_rfoot(0,3) =  footcoord.getTranslation()(0);
        T_torso_rfoot(1,0) =  footcoord.getRotation()(1,0);
        T_torso_rfoot(1,1) =  footcoord.getRotation()(1,1);
        T_torso_rfoot(1,2) =  footcoord.getRotation()(1,2);
        T_torso_rfoot(1,3) =  footcoord.getTranslation()(1);
        T_torso_rfoot(2,0) =  footcoord.getRotation()(2,0);
        T_torso_rfoot(2,1) =  footcoord.getRotation()(2,1);
        T_torso_rfoot(2,2) =  footcoord.getRotation()(2,2);
        T_torso_rfoot(2,3) =  footcoord.getTranslation()(2);
        T_torso_rfoot(3,0) =  0;
        T_torso_rfoot(3,1) =  0;
        T_torso_rfoot(3,2) =  0;
        T_torso_rfoot(3,3) =  1;
        // gget CoM in torso coordinates
        KVecDouble3 Sum_CoM_Torso = kin.calculateCenterOfMass();
        Eigen::Vector4d Sum_CoM_Torso_v;
        Sum_CoM_Torso_v(0) = Sum_CoM_Torso(0,0);
        Sum_CoM_Torso_v(1) = Sum_CoM_Torso(1,0);
        Sum_CoM_Torso_v(2) = Sum_CoM_Torso(2,0);
        Sum_CoM_Torso_v(3) = 1;

        //calculate CoM in foot coordinates
        Eigen::Vector4d Sum_CoM_RFoot = T_torso_rfoot.inverse() * Sum_CoM_Torso_v;
        //cout << "Sum_CoM_RFoot x: "<< Sum_CoM_RFoot(0) << " y: " << Sum_CoM_RFoot(1)<< endl;// << " z: " << Sum_CoM_RFoot(2) << endl;
        return Sum_CoM_RFoot;
      }
AL::ALValue kinematics_module::get_com()
{
    ALMotionProxy motion(getParentBroker());
    std::vector<float> sensorAngles = motion.getAngles("Joints", false);
    std::cout << "Sensor angles: " << std::endl << sensorAngles << std::endl;
    std::cout << "KDeviceLists::NUMOFJOINTS: " << KDeviceLists::NUMOFJOINTS << std::endl;

    NAOKinematics nkin;
    std::cout << "Set joints successful: " << nkin.setJoints(sensorAngles) << std::endl;
    KVecDouble3 com = nkin.calculateCenterOfMass();
    return AL::ALValue::array(com.get(0, 0), com.get(1, 0), com.get(2, 0));
}
Exemplo n.º 3
0
      // Transformation for 2leg balance
      Eigen::Matrix4d Tf_torso_2feet(){
        // TF Rleg-Torso
        NAOKinematics::kmatTable footcoord_R = kin.getForwardEffector((NAOKinematics::Effectors)CHAIN_R_LEG);
        Eigen::Matrix4d T_torso_rfoot;
        T_torso_rfoot(0,0) =  footcoord_R.getRotation()(0,0);
        T_torso_rfoot(0,1) =  footcoord_R.getRotation()(0,1);
        T_torso_rfoot(0,2) =  footcoord_R.getRotation()(0,2);
        T_torso_rfoot(0,3) =  footcoord_R.getTranslation()(0);
        T_torso_rfoot(1,0) =  footcoord_R.getRotation()(1,0);
        T_torso_rfoot(1,1) =  footcoord_R.getRotation()(1,1);
        T_torso_rfoot(1,2) =  footcoord_R.getRotation()(1,2);
        T_torso_rfoot(1,3) =  footcoord_R.getTranslation()(1);
        T_torso_rfoot(2,0) =  footcoord_R.getRotation()(2,0);
        T_torso_rfoot(2,1) =  footcoord_R.getRotation()(2,1);
        T_torso_rfoot(2,2) =  footcoord_R.getRotation()(2,2);
        T_torso_rfoot(2,3) =  footcoord_R.getTranslation()(2);
        T_torso_rfoot(3,0) =  0;
        T_torso_rfoot(3,1) =  0;
        T_torso_rfoot(3,2) =  0;
        T_torso_rfoot(3,3) =  1;

        // TF Lleg-Torso
        NAOKinematics::kmatTable footcoord_L = kin.getForwardEffector((NAOKinematics::Effectors)CHAIN_L_LEG);
        Eigen::Matrix4d T_torso_lfoot;
        T_torso_lfoot(0,0) =  footcoord_L.getRotation()(0,0);
        T_torso_lfoot(0,1) =  footcoord_L.getRotation()(0,1);
        T_torso_lfoot(0,2) =  footcoord_L.getRotation()(0,2);
        T_torso_lfoot(0,3) =  footcoord_L.getTranslation()(0);
        T_torso_lfoot(1,0) =  footcoord_L.getRotation()(1,0);
        T_torso_lfoot(1,1) =  footcoord_L.getRotation()(1,1);
        T_torso_lfoot(1,2) =  footcoord_L.getRotation()(1,2);
        T_torso_lfoot(1,3) =  footcoord_L.getTranslation()(1);
        T_torso_lfoot(2,0) =  footcoord_L.getRotation()(2,0);
        T_torso_lfoot(2,1) =  footcoord_L.getRotation()(2,1);
        T_torso_lfoot(2,2) =  footcoord_L.getRotation()(2,2);
        T_torso_lfoot(2,3) =  footcoord_L.getTranslation()(2);
        T_torso_lfoot(3,0) =  0;
        T_torso_lfoot(3,1) =  0;
        T_torso_lfoot(3,2) =  0;
        T_torso_lfoot(3,3) =  1;

        //CoM in torso frame
        KVecDouble3 Sum_CoM_Torso = kin.calculateCenterOfMass();
        Eigen::Vector4d Sum_CoM_Torso_v;
        Sum_CoM_Torso_v(0) = Sum_CoM_Torso(0,0);
        Sum_CoM_Torso_v(1) = Sum_CoM_Torso(1,0);
        Sum_CoM_Torso_v(2) = Sum_CoM_Torso(2,0);
        Sum_CoM_Torso_v(3) = 1;

        //calculate CoM in R and L foot frame
        Eigen::Vector4d Sum_CoM_RFoot = T_torso_rfoot.inverse() * Sum_CoM_Torso_v;
        Eigen::Vector4d Sum_CoM_LFoot = T_torso_lfoot.inverse() * Sum_CoM_Torso_v;

        //calculate position of left foot in right foot coordinates
        Eigen::Vector4d O_LFoot;
        O_LFoot << 0, 0, 0, 1;
        Eigen::Matrix4d Tf_lf_rf = T_torso_lfoot * T_torso_rfoot.inverse();
        Eigen::Vector4d O_LFoot_in_RFoot = Tf_lf_rf * O_LFoot ;
        Eigen::Vector4d O_RFoot;
        O_RFoot << 0, 0, 0, 1;

        Eigen::Matrix4d Sum_CoM_2Foot;
        Sum_CoM_2Foot << Sum_CoM_RFoot, Sum_CoM_LFoot, O_RFoot, O_LFoot_in_RFoot;
        //return CoM in left and right foot frame and coordinates of feet in right foot frame
        return Sum_CoM_2Foot;
      }