void CStaubli::MakestaubliOri(Matrix3d& rHapticOri, Vector3d& rStaubliOri)
{
//좌표계 보정부터 다시. z
//방법 2. Handbook of Robotics에서 찾은 것.. psi = alpha, theta = beta, phi = gamma
double psi = 0.0;
double theta = 0.0;
double phi = 0.0;
theta = atan2(-rHapticOri.coeff(2, 0), sqrt(rHapticOri.coeff(0, 0)*rHapticOri.coeff(0, 0) +
rHapticOri.coeff(1, 0)*rHapticOri.coeff(1, 0)));
psi = atan2(rHapticOri.coeff(1, 0)/cos(theta), rHapticOri.coeff(0, 0)/cos(theta));
phi = atan2(rHapticOri.coeff(2, 1)/cos(theta), rHapticOri.coeff(2, 2)/cos(theta));
//방.법 X-Y-Z Euler angle (refernce manual 보면 staubli 는 이거 사용 하는 것 처럼 보임)
double alpha = 0.0;
double beta = 0.0;
double gamma = 0.0;
// theta
// phi
// psi
beta = atan2(rHapticOri.coeff(0, 2), sqrt(rHapticOri.coeff(1, 2)*rHapticOri.coeff(1, 2)+rHapticOri.coeff(2, 2)*rHapticOri.coeff(2, 2)));
gamma = atan2(-rHapticOri.coeff(0, 1)/cos(beta), rHapticOri.coeff(0, 0)/cos(beta));
alpha = atan2(-rHapticOri.coeff(1, 2)/cos(beta), rHapticOri.coeff(2, 2)/cos(beta));
printf("alpha: %f, beta: %f, gamma: %f \n", alpha, beta, gamma);
//if(alpha < -180)
// alpha = alpha+360;
//else if(alpha>180)
// alpha = alpha-360;
//if(beta < -180)
// beta = beta+360;
//else if(beta>180)
// beta = beta-360;
//if(gamma < -180)
// gamma = gamma+360;
//else if(gamma>180)
// gamma = gamma-360;
double pi = 3.141592;
Vector3d omni_orihome; //이건 뭘 기준으로 가져왔더라? omni stylus를 직각으로 세워두었을 때의 값인 듯.
omni_orihome[0] = 0;
omni_orihome[1] = 0;
omni_orihome[2] = -90;
//VectorXd staubli_orihome(3);
Vector3d staubli_orihome;
staubli_orihome[0] = 0.0;
staubli_orihome[1] = -180.0;
staubli_orihome[2] = 0.0;
Vector3d ori_omni;
ori_omni[0] = alpha * 180/pi;
ori_omni[1] = beta * 180/pi;
ori_omni[2] = gamma * 180/pi;
//rStaubliOri[0] = (ori_omni[0] - omni_orihome[0]) + staubli_orihome[0];
//rStaubliOri[1] = (ori_omni[1] - omni_orihome[1]) + staubli_orihome[1];
//rStaubliOri[2] = -(ori_omni[2] - omni_orihome[2]) + staubli_orihome[2]; // z축 방향만 반대..
rStaubliOri[0] = ori_omni[0];
rStaubliOri[1] = ori_omni[1];
rStaubliOri[2] = ori_omni[2];
//for(int i=0; i<3; i++){
// if(rStaubliOri[i] < -180)
// rStaubliOri[i] = rStaubliOri[i]+360;
// else if(rStaubliOri[i]>180)
// rStaubliOri[i] = rStaubliOri[i]-360;
//}
//static int printer_count = 0;
//if (printer_count == 10){
// std::cout<<"2."<<psi * 180/pi<<" "<<theta * 180/pi<<" "<<phi * 180/pi<<std::endl; //1번 방법과 같은 지 확인 OK, 다만 tan가 각도가 다름
// std::cout<<"4."<<alpha * 180/pi<<" "<<beta * 180/pi<<" "<<gamma * 180/pi<<std::endl; // 이거는 omni의 값을 staubli 방식으로 해석한 것.
// std::cout<<rStaubliOri[0]<<" "<<rStaubliOri[1]<<" "<<rStaubliOri[2]<<std::endl;
// printer_count = 0;
//}
//printer_count++;
//std::cout<<rHapticOri.coeff(0, 0)<<" h"<<rHapticOri(1, 1)<<" "<<rHapticOri(2, 2)<<std::endl;
}
