//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));
}
int main(){
NAOKinematics *nkin = new NAOKinematics();
NAOKinematics::FKvars output1, output2, output3, output4, output5;
std::vector<double> flh,frh,fll,frl,fc,empty;
double pi = KMath::KMat::transformations::PI;
//Left Hand
flh.push_back(0);flh.push_back(pi/4);flh.push_back(-pi/3);flh.push_back(-pi/4);flh.push_back(pi/4);
output1 = nkin->filterForwardFromTo("Torso","LeftArm",empty,flh);
//Right Hand
frh.push_back(0);frh.push_back(-pi/4);frh.push_back(-pi/3);frh.push_back(pi/4);frh.push_back(pi/4);
output2 = nkin->filterForwardFromTo("Torso","RightArm",empty,frh);
//Left Leg
fll.push_back(0);fll.push_back(0);fll.push_back(0);fll.push_back(1.2);fll.push_back(0.1);fll.push_back(0);
output3 = nkin->filterForwardFromTo("Torso","LeftLeg",empty,fll);
//Right Leg
frl.push_back(-pi/10);frl.push_back(0);frl.push_back(0);frl.push_back(pi/2);frl.push_back(0);frl.push_back(0);
output4 = nkin->filterForwardFromTo("Torso","RightLeg",empty,frl);
//Camera
fc.push_back(pi/2);fc.push_back(pi/3);
output5 = nkin->filterForwardFromTo("Torso","CameraBot",empty,fc);
std::cout << "x = " << output1.pointX << " y = " << output1.pointY << " z = " << output1.pointZ << " ax = " << output1.angleX << " ay = " << output1.angleY << " az = " << output1.angleZ << std::endl;
std::cout << "x = " << output2.pointX << " y = " << output2.pointY << " z = " << output2.pointZ << " ax = " << output2.angleX << " ay = " << output2.angleY << " az = " << output2.angleZ << std::endl;
std::cout << "x = " << output3.pointX << " y = " << output3.pointY << " z = " << output3.pointZ << " ax = " << output3.angleX << " ay = " << output3.angleY << " az = " << output3.angleZ << std::endl;
std::cout << "x = " << output4.pointX << " y = " << output4.pointY << " z = " << output4.pointZ << " ax = " << output4.angleX << " ay = " << output4.angleY << " az = " << output4.angleZ << std::endl;
std::cout << "x = " << output5.pointX << " y = " << output5.pointY << " z = " << output5.pointZ << " ax = " << output5.angleX << " ay = " << output5.angleY << " az = " << output5.angleZ << std::endl;
vector<vector<double> > result;
result = nkin->inverseLeftHand(output1.pointX,output1.pointY,output1.pointZ,output1.angleX,output1.angleY,output1.angleZ);
if(!result.empty())
cout << "--Solution exists 1" << endl;
result = nkin->inverseRightHand(output2.pointX,output2.pointY,output2.pointZ,output2.angleX,output2.angleY,output2.angleZ);
if(!result.empty())
cout << "--Solution exists 2" << endl;
result = nkin->inverseLeftLeg(output3.pointX,output3.pointY,output3.pointZ,output3.angleX,output3.angleY,output3.angleZ);
if(!result.empty())
cout << "--Solution exists 3" << endl;
result = nkin->inverseRightLeg(output4.pointX,output4.pointY,output4.pointZ,output4.angleX,output4.angleY,output4.angleZ);
if(!result.empty())
cout << "--Solution exists 4" << endl;
return 0;
}
// BALANCING
bool CoM_chk(){
while (current_state.position.size() == 0){}
sensor_msgs::JointState chk_state;
chk_state = current_state;
for (int v = 0; v < desired_states.position.size(); v++)
{
for(int u = 0; u < chk_state.position.size(); u++)
{
if(chk_state.name[u] == desired_states.name[v])
{
chk_state.position[u] = desired_states.position[v];
break;
}
}
}
std::vector<float> jointanglesdes(chk_state.position.begin(), chk_state.position.end());
kin.setJoints(jointanglesdes);
if (bal_RLeg){
//cout << "1leg stance balance: "<< chk_CoM_RLeg()[0] << endl;
if (chk_CoM_RLeg()[0]) return true;
else return false;
}
else if (bal_2Leg){
//cout << "2leg stance balance: " << chk_CoM_2legs() << endl;
if(chk_CoM_2legs()) return true;
else return false;
}
else return true;
}
// 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;
}
