unsigned char RobotSim::getCurrentConfiguration()
{
vector<double> q;
unsigned char conf;
//save current joint values and update the joint values
for(int i=0;i<(int)joints.size();i++)q.push_back(joints[i]->getValue());
getConfigurationOf(q,conf);
return conf;
}
bool EUITIbotSim::EUITIbotInverseKinematics(Transformation3D t06, vector<double> &_q, unsigned char _conf)
{
//*****************************************
vector<double> q_act;
unsigned char c_act;
getCoordinatesOf(q_act);//Save current coordiantes
getConfigurationOf(q_act,c_act);
//*****************************************
double L1=0.09550;
double L2=0.108;
double L3=0.128;
double L6=0.103;
if(_conf == 0x00)
_conf=getCurrentConfiguration();
double _s, _e;
configuration(_conf,_s,_e);
Vector3D c = t06.position - t06.getVectorW()*(L6);
//THETA 1
double theta1 = atan2(c.y,c.x);
if (theta1<0)theta1 = 2*PI + theta1;
//THETA 2
double theta2;
double m = sqrt(c.x*c.x + c.y*c.y + c.z*c.z);
double r = sqrt(c.x*c.x + c.y*c.y + (c.z-L1)*(c.z-L1));
double alpha = acos((L1*L1 + r*r - m*m )/2*r*L1);
double beta = atan((L2*L2 + r*r - L3*L3 )/2*r*L2);
if (r + L2 == m)theta2 = PI/2 - beta;
else if (L2 + L3 == r)theta2 = alpha - PI/2;
else if (L1 + L2 + L3 == m)theta2 = PI/2;
else {theta2 = alpha - beta - PI/2;}
//THETA 3
double theta3;
double gamma = acos((L3*L3 + L2*L2 - r*r )/2*L2*L3);
if(L2 + L3 == r)theta3 = PI/2;
else {theta3 = 3*PI/2 - gamma;}
//DESACOPLO CINEMATICO
OrientationMatrix m06,m03,m36;
Matrix3x3 &aux03 = m03;
m06=t06.orientation;
//Meto las 3 primeras coordenadas finales y corregidas
setJointValue(0,theta1);
setJointValue(1,theta2);
setJointValue(2,theta3);
Transformation3D auxm = joints[3]->getAbsoluteT3D();
//Devuelvo la posicion previa
for(int i=0;i<6;i++)
setJointValue(i,q_act[i]);
aux03=auxm.orientation;
m36=(m03.inverted())*m06;
//THETA 4
double theta4 = asin(m36[2][3]/m36[3][3]);
//THETA 5
double theta5 = acos(m36[3][3]);
//THETA 6
double theta6 = atan(m36[3][2]/-m36[3][1]);
//CONFIGURACIONES
_q.push_back(theta1);
_q.push_back(theta2);
_q.push_back(theta3);
_q.push_back(theta4);
_q.push_back(theta5);
_q.push_back(theta6);
for(int i=0;i<6;i++){if(fabs(_q[i])<EPS){_q[i]=0;}}
return true;
}
