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