float NaoHeadControl::headPositionDistanceToActualPosition(Vector3f comp,bool leftSide) { float pos; pos = gRadToDeg(NAO->GetLink()[Nao::JID_HEAD_1].q) ; return -gAbs(pos-comp.y()); }
void Effectors::calculateVel(EJointID id, float angle, float maxVel) { if (maxVel < 0) { std::cout << "calculateVel Error"; mJointVel[id] = 0.0f; return ; } float curAngle = gRadToDeg(mJointAngle[id]); float minus = gNormalizeDeg(angle - curAngle); float vel = 0.0; vel = gAbs(minus) > maxVel ? maxVel * gSign(minus) : minus; vel = std::min(gDegToRad(vel) * 20.0f, 200.0f); mJointVel[id] = vel; }