[Aiken]

Muy buenas a tod@s!!

Soy nuevo en el foro, me dedico a la programación y soy nulo en mates, de hecho me ha costado elegir donde meter este tema, la cosa es que llevo ya una semana con este problema y no veo luz por ningún lado, espero que me podais dar alguna pista, de antemano muchas gracias por vuestro tiempo.

Lo que pretendo es crear un sistema de coordenadas a partir de tres puntos conocidos, para ello lo que hago es:

Defino 3 puntos P1, p2 y p3
P1, define el origen del sistema.
P2, define el vector del eje X.
P3, define el plano XY.

Calculo vector en eje X
\( \overrightarrow{VX} = P2 - P1 \)

vector auxiliar para calcular VY
\( \overrightarrow{VA} = P3 - P1 \)

Vector en eje Y
\( \overrightarrow{VY} = \overrightarrow{VX} * \overrightarrow{VA} \)

y vector en eje z
\( \overrightarrow{VZ} = \overrightarrow{VY} * \overrightarrow{VX} \)

Calcúlo los vectores unitarios para formar la matriz de transformación
\(
Matriz Rot = \begin{bmatrix}{VXnorm.x}&{VYnorm.x}&{VZnorm.x}\\{VXnorm.y}&{VYnorm.y}&{VZnorm.y}\\{VXnorm.z}&{VYnorm.z}&{VZnorm.z}\end{bmatrix}
 \)

Hasta aqui creo que voy bien, ahora necesito obtener los angulos sobre cada eje para hacer las rotaciones.
Segun leo en http://www.songho.ca/opengl/gl_anglestoaxes.html una de las muchas (pero muchas) páginas que he consultado, para un orden de rotaciones ZXY (es el que necesito) tengo que aplicar las siguientes ecuaciones:

EJE Y:
\( VXnorm.z = -Sin(B) \) ; B = rot en Y, por lo tanto
\( rot Y = 90 - arcCos(-VXnorm.z)  \), aqui tendria que ser arcSin(-VXnorm.z), pero el sistema donde tengo que implementar la programación no me permite la función arcoseno y por lo que he visto así también se puede calcular.

EJE X:
\( VZnorm.z = Cos(B)Cos(A) \) ; A = rot en X, por lo tanto

\( \displaystyle rot X = arcCos(\frac{VZnorm.z}{Cos(rot Y)})  \)

EJE Z:
\( VXnorm.x = Cos(C)Cos(B) \) ; C = rot en Z, por lo tanto

\( \displaystyle rot Z = arcCos(\frac{VXnorm.x}{Cos(rot Y)})  \)

Es esto correcto?, esta bien aplicado?

Uso un simulador 3d para comprobar los calculos, y me dan resultados diferentes, por ejemplo:

P1 = -278, 182, 45
P2 = -230, 178, -30
P3 = -210, 160, -10

la matriz de rotacion resultante es:

\(
Matriz Rot = \begin{bmatrix}
{0.538511}&{-0.484505}&{0.689392}\\
{-0.0448759}&{-0.833484}&{-0.550718}\\
{-0.841423}&{-0.265631}&{0.470583}\end{bmatrix}
 \)

El programa de simulación que utilizo tiene una función que hace esto mismo que pretendo hacer y me da los siguientes resultados:
rX = 119.44
rY = 57.29
rZ = -4.76

Y según mis calculos:
rX = 29.44, muy diferente, coincide que 119.44 - 90 = 29.44, sospechoso, pero ni idea
rY = 57.29, este valor siempre me da bien
rZ = 4.76, mismo valor pero con signo cambiado

Intuyo que tiene algo que ver con que cos(20) = cos(340), es decir que tengo el mismo resultado de coseno para dos valores distintos, pero.. ¿en función de qué modifico el valor de los resultados??
 

Cualquier ayuda o aclaración me seria de gran ayuda, perdón por el tocho pero no sabia como resumirlo

Muchas gracias y saludos,

[Masacroso]

¿Un sistema de coordenadas ortonormal de un espacio tridimensional? Entonces eso es una traslación y rotación del sistema de coordenadas estándar.

La traslación viene dada por \( P_1 \) y la rotación sería una matriz \( 3\times 3 \) de cambio de base definida por el cambio de base desde la base ortonormal estándar \( e_1:=(1,0,0),\, e_2:=(0,1,0) \) y \( e_3:=(0,0,1) \) a tu base particular \( v_1,v_2,v_3 \).

Asumo tu base ortonormal, por simplificar, entonces \( v_1=\overline{P_1P_2}/\|\overline{P_1P_2}\| \), donde \( \|\overline{P_1P_2}\| \) es el módulo del vector \( \overline{P_1P_2} \), dividimos por el módulo para normalizar el vector.

Ahora bien si definimos \( v_2:=\overline{P_1P_3} \) primero debemos verificar que \( v_1 \) y \( v_2 \) son linealmente independientes, lo que nos asegura que los dos vectores (junto a \( P_1 \)) definen un plano en \( \Bbb R^3 \). Eso se verifica viendo que no existe \( \lambda\in\Bbb R \) tal que \( \lambda v_2=v_1 \).

Una vez hecho queremos hallar un vector ortonormal a \( v_1 \), llamémoslo \( \tilde v_2 \), tal que entre los dos definan el mismo plano. Eso se puede hacer usando el algoritmo de Gram-Schmidt, es decir hallando \( \tilde v_2=\frac{v_2-(v_2\mid v_1)v_1}{\|v_2-(v_2\mid v_1)v_1\|} \).

Hallados ya \( v_1,\tilde v_2 \) para hallar un tercer vector ortonormal tomamos cualquier punto no contenido en el plano, llamémoslo \( P_4 \), definiendo \( v_3:=\overline{P_1P_4} \) hallamos un tercer vector ortonormal aplicando de nuevo el algoritmo de Gram-Schmidt:

\( \displaystyle \tilde v_3=\frac{v_3-( v_3\mid\tilde v_2)\tilde v_2-(v_3\mid v_1)v_1}{\|v_3-( v_3\mid\tilde v_2)\tilde v_2-(v_3\mid v_1)v_1\|} \)

Entonces el sistema de vectores ortonormales \( v_1,\tilde v_2,\tilde v_3 \) junto con el punto \( P_1 \) define tu sistema de coordenadas de esta manera: supongamos que tienes un punto \( Q \) en las coordenadas estándar y lo quieres pasar a tu nuevo sistema de coordenadas, entonces las coordenadas del punto en este nuevo sistema serían \( (Q')^T=A^{-1}(\overline{P_1Q})^T \), donde \( A \) es una matriz \( 3\times 3 \) donde sus columnas son \( v_1,\tilde v_2,\tilde v_3 \), y \( \overline{P_1Q}^T \) es el vector \( \overline{P_1Q} \) traspuesto.

Para completar los cálculos añadir que si \( x=(a,b,c) \) y \( y=(d,e,f) \) entonces \( (x\mid y)=ad+be+cf \), es decir, el producto escalar que conoces, y por tanto \( (x\mid y)y \) es el vector \( y \) multiplicado por ese escalar. Y también aclarar que \( \|x\|=\sqrt{a^2+b^2+c^2} \) y \( \overline {PQ}=Q-P \).

Si tienes alguna duda pregunta.

[Aiken]

Muy buenas, gracias por responder Masacroso,

Lo que comentas de la base ortonormal, pues realmente se me escapa, la idea es crear un sistema de coordenas con la siguiente definición:

Sis_Coord = {X, Y, Z, A, B, C}

Donde X Y y Z son las coordendas del origen del nuevo sistema respecto a un sistema base, y A B y C son las rotacion sobre el eje Z, el eje Y y el eje X respectivamnete y por ese orden.

El procedimiento para hallar lo he sacado de esta pagina
http://www.robot-forum.com/robotforum/kuka-robot-forum/abc-3-point-base-measurement-how-does-algorithm-works/
donde se habla del tema, entiendo que la primera parte del procemiento donde se obtiene la matriz de rotación la tengo bien, pero no lo tengo claro.

Y la que no consigo de ninguna manera, es la parte donde obtiene los angulos de la matriz de rotación, he visto muchisimos sitios y no he sacado nada claro.
La rotación sobre Y si que me sale siempre bien, pero X y Z no.

También lo explica en esta página de una manera aparentemente sencilla
http://javakuka.com/xyzabc/
pero me ha dejado más confundido aún,

Saludos y gracias de nuevo,

[Masacroso]

Muy buenas, gracias por responder Masacroso,

Lo que comentas de la base ortonormal, pues realmente se me escapa, la idea es crear un sistema de coordenas con la siguiente definición:

Sis_Coord = {X, Y, Z, A, B, C}

Donde X Y y Z son las coordendas del origen del nuevo sistema respecto a un sistema base, y A B y C son las rotacion sobre el eje Z, el eje Y y el eje X respectivamnete y por ese orden.

El procedimiento para hallar lo he sacado de esta pagina
http://www.robot-forum.com/robotforum/kuka-robot-forum/abc-3-point-base-measurement-how-does-algorithm-works/
donde se habla del tema, entiendo que la primera parte del procemiento donde se obtiene la matriz de rotación la tengo bien, pero no lo tengo claro.

Y la que no consigo de ninguna manera, es la parte donde obtiene los angulos de la matriz de rotación, he visto muchisimos sitios y no he sacado nada claro.
La rotación sobre Y si que me sale siempre bien, pero X y Z no.

También lo explica en esta página de una manera aparentemente sencilla
http://javakuka.com/xyzabc/
pero me ha dejado más confundido aún,

Saludos y gracias de nuevo,

Aquí

http://www.robot-forum.com/robotforum/kuka-robot-forum/abc-3-point-base-measurement-how-does-algorithm-works/

describe lo mismo que yo he hecho, con la diferencia de que tendría que comprobar que el algoritmo Gram-Schmidt usado define la misma orientación de \( \tilde v_2 \) y \( \tilde v_3 \) que el producto vectorial. En cualquier caso puedes usar el producto vectorial en vez del procedimiento Gram-Schmidt y ya está, es decir \( n_3:=\frac{v_1\times v_2}{\|v_1\times v_2\|} \) es un vector ortonormal al plano definido por \( v_1 \) y \( v_2 \), entonces defines \( n_2:=\frac{n_3\times v_1}{\|n_3\times v_1\|} \) y tienes una base ortonormal \( v_1,n_2,n_3 \) (que es la misma que \( v_1,\tilde v_2,\tilde v_3 \) excepto quizá el signo de los últimos dos vectores).

Entonces la matriz de rotación se define por las columnas que son esos vectores, y a partir de ahí supuestamente se deben hallar los ángulos de Euler aunque no tengo mucha idea de cómo se hace (la página de la wikipedia en inglés es algo confusa y además hay bastantes convenciones involucradas, lo que oscurece un poco el asunto).

Parece que aquí viene algo mejor explicado:

http://javakuka.com/xyzabc/

pero es un poco coñazo ponerse a estudiar todo esto, lo siento, no soy fan del cálculo. A lo mejor algún ingeniero del foro te echa un cable. En cualquier caso te queda por averiguar como hallar los valores A, B y C conociendo la matriz de rotación.

[Aiken]

ese es el tema, hallar los ángulos de euler a partir de una matriz de rotación, creo que me estoy metiendo en un jardín que me tiene desvelado ...
 

pero seguiré intentandolo, si averiguo algo volveré a dejar constancia por si le sirve a alguien.

[Masacroso]

Creo que ya entiendo la notación... Los ángulos de Euler se definen por orden de rotación a través de cada eje en el nuevo sistema después de cada giro. En tu caso, según describe @panic mode en uno de los enlaces tenemos que \( A=Z_1,\, B= Y_2,\, C=X_3 \).

Si tienes la matriz de rotación y acceso a Wolfram Mathematica entonces hallas esos valores con el código "EulerAngles[r,{3,2,1}]" donde r es la matriz de rotación antes descrita. Lo he mirado aquí.

Luego con más ganas a lo mejor me pongo a mirar cómo se hace el cálculo manualmente.

[Abdulai]

........
Lo que pretendo es crear un sistema de coordenadas a partir de tres puntos conocidos, para ello lo que hago es:

Defino 3 puntos P1, p2 y p3
P1, define el origen del sistema.
P2, define el vector del eje X.
P3, define el plano XY.

Calculo vector en eje X
\( \overrightarrow{VX} = P2 - P1 \)

vector auxiliar para calcular VY
\( \overrightarrow{VA} = P3 - P1 \)

Vector en eje Y
\( \overrightarrow{VY} = \overrightarrow{VX} * \overrightarrow{VA} \)

y vector en eje z
\( \overrightarrow{VZ} = \overrightarrow{VY} * \overrightarrow{VX} \)

Están cambiados los ejes, debe ser:

Vector en eje z
\( \overrightarrow{VZ} = \overrightarrow{VX} * \overrightarrow{VA} \)

y vector en eje y
\( \overrightarrow{VY} = \overrightarrow{VZ} * \overrightarrow{VX} \)

La matriz de rotación que calculás mas adelante termina siendo la misma pero con dos columnas intercambiadas.

\(
Matriz Rot = \begin{bmatrix}
{0.538511}  & {0.689392}  & {-0.484505} \\
{-0.0448759}& {-0.550718} & {-0.833484} \\
{-0.841423} & {0.470583}  & {-0.265631} \end{bmatrix}
 \)
 

................
................
EJE X:
\( VZnorm.z = Cos(B)Cos(A) \) ; A = rot en X, por lo tanto

\( \displaystyle rot X = arcCos(\frac{VZnorm.z}{Cos(rot Y)})  \)

No es una buena elección porque el coseno es un función par y arcCos te devuelve solo valores positivos.
Tenés que usar \( VXnorm.y = \sin C \cos B \)

EJE Z:
\( VXnorm.x = Cos(C)Cos(B) \) ; C = rot en Z, por lo tanto
\( \displaystyle rot Z = arcCos(\frac{VXnorm.x}{Cos(rot Y)})  \)

Acá te va a pasar lo mismo que en el caso anterior cuando el ángulo sea negativo.  Ahora te da bien porque es positivo (habiendo corregido el tema de las columnas)

[Aiken]

Muchas gracias Abdulai!

Es cierto estaba cambiado el eje Z por el Y, y por eso me daba resultados erroneos.
Respecto a lo que comentas de utilizar la ecuación con arcosenos, tampoco funciona bien, según en que cuadrante esten definidos los puntos P1, p2 y P3,  en el eje Z a veces tengo que cambair el signo y en el eje X a veces tengo que sumar 90º.

Una vez corregido el problema he hecho pruebas utilizando éstas ecuaciones con tangentes (las vi en un video de youtube a altas horas de la noche y no tengo el enlace):

\( \begin{bmatrix}{a}&{b}&{c}\\{d}&{e}&{f}\\{g}&{h}&{i}\end{bmatrix} \Rightarrow{rX, rY, rZ} \)

\(  rX = arcTan2(h,i) \)
\(  rY = arcSin(-g) \)
\(  rZ = arcTan2(d,a) \)

He hecho pruebas con diferentes posiciones de los puntos y los resultados estan bien, pero no sé si habría algún problema cuando alguno de los ejes del nuevo sistema es paralelo al del sistema de referencia, habría que probar, en mi caso no me preocupa por que no se va a dar el caso.

Gracias por el aporte, saludos,