Como la deducción de estas fórmulas es bastante largo de escribir y explicar ---> la dejo pendiente
Si \( a=SPA,b=SPB \) y \( c=SPC \) son los de rotación en el 530, en el 640 serán:
\( SPA_{640} = \text{atan}\left(-\displaystyle\sqrt{\sin(a)^2 \cos(b)^2+\sin(b)^2}\;,\;\cos(a) \cos(b)\right) \)
\( SPB_{640} = 0 \)
\( SPC_{640} = \text{atan}\left(-(\cos(a)\sin(b)\cos(c)+sin(a)\sin(c)) \;,\; \cos(a)\sin(b)\sin(c)-\sin(a)\cos(c)\right) \)
Ojo! La función \( \text{atan}(y,x) \) te
devuelve el ángulo en los 4 cuadrantes (en Matlab y C es atan2(y,x) )
El ángulo de rotación de las trayectorias: \( \theta = \text{atan}\left(-\sin(b)\;,\;-\sin(a)\cos(b)\right) \)
Y su matriz de rotación: \( R_{xy} = \begin{bmatrix}\cos\theta & -sin\theta & 0\\sin\theta & \cos\theta & 0\\0 & 0 & 1\end{bmatrix} \)
Es decir, en las trayectorias siguientes hay que transformar:
\( X_{640} = \cos(\theta) X_{530}-\sin(\theta) Y_{530} \)
\( Y_{640} = \sin(\theta) X_{530}+\cos(\theta) Y_{530} \)
--------------------------------------------------------------------------------
Del 640 al 530 no sería necesario convertir nada, pues te sobra 1 grado de libertad.
Salvo que convengan giros arbitrarios en el eje B para una mejor orientación de la mesa. Aunque no me queda claro que criterio aplicar.
--------------------------------------------------------------------------------
Por las dudas probé en MAtlab con los ángulos de tu 1er mensaje
%Angulos V530 en grados
SPA = +84.355 ;
SPB = -28.881 ;
SPC = -1.216 ;
%------------------------------------------
kd=pi/180 ; %Constante grados->radianes
a=kd*SPA ; b=kd*SPB ; c=kd*SPC ;
w = atan2(-sin(b),-sin(a)*cos(b)) ;
s = atan2(-sqrt(sin(a)^2*cos(b)^2+sin(b)^2),cos(a)*cos(b)) ;
t = atan2(-(cos(a)*sin(b)*cos(c)+sin(a)*sin(c)),cos(a)*sin(b)*sin(c)-sin(a)*cos(c)) ;
%------------------------------------------
%Angulos V640 y giro XY en grados
[s/kd ,0, t/kd ] , w/kd
ans =
-85.0589691824008 0 176.050728853882
ans =
151.001049676576
Saludos.
