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