Hola, perdón por el retraso.
La librería de complejos no he conseguido sabe usarla. Pero creo que he limitado un poco el problema. Estaba cometiendo el error de no tomar las distancias con valores absolutos.
Ahora mismo me siguen dando complejos en algunos puntos pero creo que tiene que ver con los puntos singulares del robot. Y estoy un poco perdida de como solucionarlo. Dejo el codigo completo de la cinemática para ver si le veis algun fallo.
//CINENMÁTICA INVERSA
void CinInversa(double x, double y, double z, double gamma, int modo, double _q[])
{
// Límites de los ángulos según CIN_DIRECTA y servos ideales
// q1=[0,pi]=[0,180]
// q2=[0,pi]=[0,180]
// q3=[-pi/2,pi/2]=[0,180]
// q4=[-pi/2,pi/2]=[0,180]
// q5=[-pi/2,pi/2]=[0,180]
//Primer angulo
_q[0]=arctg(y,x);
//Tercer ángulo
double r_p=sqrt(sq(x)+sq(y));
double y_p=abs(z);
double gamma_rad=gamma*(PI/180);
double Afr=abs((d4+d5)*cos(gamma_rad));
double val1=max(r_p,Afr);
double val2=min(r_p,Afr);
double LadoR=val1-val2;
double Afy=((d4+d5)*sin(gamma_rad));
double val3=max(y_p,Afy);
double val4=min(y_p,Afy);
double LadoY=val3-val4-d1;
double hipotenusa=abs(sqrt(sq(LadoR)+sq(LadoY)));
// if (hipotenusa>d2+d3) //Esta condición funciona a veces. En ocasiones sale que la hipotenusa es mayor de
// hipotenusa=d2+d3; // lo que fisicamente puede ser y es por lo que sale complejos
// hipotenusa^2= d2^2+d3^2+2d2d3*cos(180-q3) (Th.coseno)
double A=(sq(hipotenusa)-sq(d2)-sq(d3))/(2d2d3); //A=cosq3
//modo vale o 1(q2 ang pequeño) o -1 (q2 ang mas grande)
double m=(modo*(sqrt(1-sq(A)))); //m=senq3
_q[2]=arctg(m,A);
//Segundo ángulo
double beta=arctg(LadoY,LadoR);
double alpha=arctg(d3sin(_q[2]),d2+d3cos(_q[2]));
_q[1]=beta-alpha;
//Cuarto ángulo
_q[3]=-_q[1]-_q[2]+gamma_rad;
//Quinto ángulo
_q[5]=0;
//Resultado en grados
_q[0]=_q[0]*180/PI;
_q[1]=_q[1]*180/PI;
_q[2]=_q[2]*180/PI;
_q[3]=_q[3]*180/PI;
_q[4]=_q[4]*180/PI;
//Comprobación de que los ángulos están en el rango válido según D-H
double min[5]={0,0,-90,-90,-90};
double max[5]={180,180,90,90,90};
for (int i=0; i<5; i++){
if ((q<min)||(q_>max*))
{
Serial.print("q");Serial.print(i);Serial.print("=");Serial.println(q);
if (isinf(q)==1.0){
Serial.println(x); Serial.println(y);Serial.println(z); Serial.println(modo); Serial.println(q);_
CinInversa(x, y, z+0.1, gamma, modo, q);}*
* //Con esto intento evitar los complejos pero no funciona. Lo que pretendia era que al encontrar*
* //una posicion invalida cambiara un poco la posicion para evitarla*
* }*_
* }*
}
[/quote]
