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