//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;
}
// 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;
}