Upload
jerry-rodriguez-flores
View
76
Download
15
Embed Size (px)
Citation preview
PRACTICA 1 DE ROBTICA TRANSFORMACIONES ESPACIALES Y MATRIZ DE ROTACIN
REPRESENTACIN DE LA ROTACIN Y TRASLACINa) Usando la funcin frame, represente una rotacin y traslacin nula>> TA = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];>> frame(TA,'c',1);>> axis([-0.5 1 -0.5 1 -0.5 1])>> grid on>> view(-33,16)
b) Representar una rotacin de 90 grados en el eje Y>> T = fix([roty(pi/2) [0;0;0];0 0 0 1])
T =
0 0 1 0 0 1 0 0 -1 0 0 0 0 0 0 1
>> frame(TA,'c',1);>> frame(T,'r',1);>> axis([-0.5 1 -0.5 1 -1 1])>> grid on>> view(-33,18)
c) Representar una rotacin de -90 en el eje Z>> T = fix([rotz(-pi/2) [0;0;0];0 0 0 1])
T =
0 1 0 0 -1 0 0 0 0 0 1 0 0 0 0 1
>> frame(TA,'c',1);>> frame(T,'r',1);>> axis([-0.5 1 -0.5 1 -0.5 1])>> grid on>> view(-25,52)
d) Representar una traslacin de 0.5 en el eje X>> T = transl(0.5,0,0);>> frame(TA,'c',1);>> frame(T,'r',1);>> axis([-0.5 1.5 -0.5 1 -0.5 1])>> grid on>> view(-30,30)
e) Representar una traslacin de 0.5 en el eje X, luego una rotacin de 90 en el eje Y, y posteriormente una rotacin de -90 en el eje Z (todos en el sistema resultante)>> T = [roty(pi/2)*rotz(-pi/2) [0.5;0;0];0 0 0 1]
T =
0.0000 0.0000 1.0000 0.5000 -1.0000 0.0000 0 0 -0.0000 -1.0000 0.0000 0 0 0 0 1.0000
>> frame(TA,'c',1);>> frame(T,'r',1);>> axis([-0.5 1.5 -0.5 1 -1 1])>> grid on>> view(-53,22)
f) Representar una matriz homognea que describa:1. Traslacin de (2,4,0) y rotacin en el eje X en 302. Traslacin de (-1,-3,2) y rotacin en el eje Z en 60>> TB = [rotx(degtorad(30)) [2;4;0];0 0 0 1]
TB =
1.0000 0 0 2.0000 0 0.8660 -0.5000 4.0000 0 0.5000 0.8660 0 0 0 0 1.0000
>> TC = [rotz(degtorad(60)) [-1;-3;2];0 0 0 1]
TC =
0.5000 -0.8660 0 -1.0000 0.8660 0.5000 0 -3.0000 0 0 1.0000 2.0000 0 0 0 1.0000
>> frame(TA,'c',1);>> frame(TB,'b',1);>> frame(TC,'m',1);>> axis([-2 3 -2 3 -2 3])>> grid on>> view(-54,22)
Ahora utilizamos la funcin rt2tr(), que nos permite obtener una matriz homognea a partir de la matriz de rotacin R y el vector de traslacin t (de dimensiones 3x1):TR = rt2tr(R, t)
>> TB = rt2tr(rotx(degtorad(30)),[2 4 0]')
TB =
1.0000 0 0 2.0000 0 0.8660 -0.5000 4.0000 0 0.5000 0.8660 0 0 0 0 1.0000
>> TC = rt2tr(rotz(degtorad(60)),[-1 -3 2]')
TC =
0.5000 -0.8660 0 -1.0000 0.8660 0.5000 0 -3.0000 0 0 1.0000 2.0000 0 0 0 1.0000
>> frame(TA,'c',1);>> frame(TB,'b',1);>> frame(TC,'m',1);>> axis([-2 3 -2 3 -2 3])>> grid on>> view(-54,22)
OPERACIONES CON TRANSFORMACIONESa) Cules son las nuevas coordenadas de p1(2,1,3) si ste pertenece a un cuerpo rgido que ha sufrido una rotacin de 90 en el eje Y?>> R = roty(degtorad(90))
R =
0.0000 0 1.0000 0 1.0000 0 -1.0000 0 0.0000
>> P1 = [2 1 3]
P1 =
2 1 3
>> P1_ = (R*P1')'
P1_ =
3.0000 1.0000 -2.0000b) Cules son las nuevas coordenadas de P1(2,1,3) si ste ha sufrido una traslacin de 2 en el eje X?>> P1 = [2 1 3]; %Definimos el vector P1>> t = transl(2,0,0); %Definimos la traslacin efectuada>> P1 = t*[P1 1]'; %Aplicamos la traslacin al vector>> P_1 = (P1(1:3))'%Cogemos solo los 3 primeros elementos del%vector resultante
P_1 =
4 1 3c) Cules son las nuevas coordenadas de P1(2,1,3) si ste ha sufrido una traslacin de 2 en el eje X y luego una rotacin de 90 en el eje Y>> P1 = [2 1 3];%Definimos el vector P1>> t = transl(2,0,0);%Definimos la traslacin efectuada>> R = roty(degtorad(90));%Definimos la rotacin realizada>> P1 = t*[P1 1]';%Aplicamos la traslacin al vector>> P1 = (P1(1:3))'%Cogemos solo los 3 primeros elementos del%vector resultante
P1 =
4 1 3
>> P1_ = (R*P1')'%Aplicamos la rotacin
P1_ =
3.0000 1.0000 -4.0000
d) Cules son las nuevas coordenadas de P1(10,7,6) si ste ha sufrido una traslacin de (10,5,0), luego una rotacin de -90 en el eje Y, luego una rotacin de 90 en el eje X?>> P1 = [10 7 6];%Definimos el vector P1>> t = transl(10,5,0);%Definimos la traslacin t>> P1 = t*[P1 1]';%Aplicamos la traslacin>> P1 = (P1(1:3))'%Nos quedamos solo con los 3 primeros%elementos del vector resultante
P1 =
20 12 6
>> P1_ = (roty(degtorad(-90))*rotx(degtorad(90))*P1')'
P1_ =
-12.0000 -6.0000 20.0000PROPIEDADES DE LA MATRIZ DE ROTACINa) Consecuencia del movimiento de un robot de 3gdl se consigui las siguientes matrices de rotacin:
Para :1. Halle su determinante>> R1 = [0.809139 -0.101016 0.57887-0.0009527 0.984886 0.173199-0.587616 -0.140694 0.796814]
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> D1 = det(R1) %Hallamos el determinante
D1 =
1.0000
2. Verifique que
>> inv(R1) %Hallamos la inversa R1^1 de la matriz de rotacin
ans =
0.8091 -0.0010 -0.5876 -0.1010 0.9849 -0.1407 0.5789 0.1732 0.7968
>> R1' %Hallamos la transpuesta R1' de la matriz de rotacin
ans =
0.8091 -0.0010 -0.5876 -0.1010 0.9849 -0.1407 0.5789 0.1732 0.7968Como se puede ver, la inversa y la transpuesta son iguales.PRACTICA 1 DE ROBTICA TRANSFORMACIONES ESPACIALES Y MATRIZ DE ROTACIN3. Si se considera sus columnas (o filas) de R como y , verifique que:
JOS CHOQUEHUAITA MACHACAPgina 8
Tomaremos las filas por lo que comprobaremos las condiciones de la segunda columna>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> r1 = R1(1,:)
r1 =
0.8091 -0.1010 0.5789
>> r2 = R1(2,:)
r2 =
-0.0010 0.9849 0.1732
>> r3 = R1(3,:)
r3 =
-0.5876 -0.1407 0.7968Corroboramos que >> r1*r1'
ans =
1.0000
>> r2*r2'
ans =
1.0000
>> r3*r3'
ans =
1.0000
Corroboramos que >> fix(r1*r2')
ans =
0
>> fix(r1*r3')
ans =
0
>> fix(r2*r3')
ans =
0Nota: utilizamos la funcin fix() para redondear el resultado, ya que MATLAB nos da respuestas en notacin cientfica.Corroboramos que >> skew1_r1 = [0 -r1(3) r1(2);r1(3) 0 -r1(1);-r1(2) r1(1) 0]
skew1_r1 =
0 -0.5789 -0.1010 0.5789 0 -0.8091 0.1010 0.8091 0
>> (skew1_r1*r2')'
ans =
-0.5876 -0.1407 0.7968
>> r3
r3 =
-0.5876 -0.1407 0.7968
Para :a) Halle su determinante>> R2 = [0.433 -0.436 0.7890.750 0.660 -0.047-0.5 0.612 0.612]
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> D1 = det(R2) %Hallamos el determinante
D1 =
0.9998
b) Verifique que >> inv(R2) %Hallamos la inversa R2^-1 de la matriz de rotacin
ans =
0.4328 0.7499 -0.5004 -0.4356 0.6597 0.6123 0.7892 -0.0470 0.6129
>> R2' %Hallamos la transpuesta R2' de la matriz de rotacin
ans =
0.4330 0.7500 -0.5000 -0.4360 0.6600 0.6120 0.7890 -0.0470 0.6120Como se puede ver, la inversa y la transpuesta son iguales.PRACTICA 1 DE ROBTICA TRANSFORMACIONES ESPACIALES Y MATRIZ DE ROTACINc) Si se considera sus columnas (o filas) de R como y , verifique que:
JOS CHOQUEHUAITA MACHACAPgina 15
Tomaremos las filas por lo que comprobaremos las condiciones de la segunda columna>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> r1 = R2(1,:)
r1 =
0.4330 -0.4360 0.7890
>> r2 = R2(2,:)
r2 =
0.7500 0.6600 -0.0470
>> r3 = R2(3,:)
r3 =
-0.5000 0.6120 0.6120
Corroboramos que >> r1*r1'
ans =
1.0001
>> r2*r2'
ans =
1.0003
>> r3*r3'
ans =
0.9991Corroboramos que >> fix(r1*r2')
ans =
0
>> fix(r1*r3')
ans =
0
>> fix(r2*r3')
ans =
0Nota: utilizamos la funcin fix() para redondear el resultado, ya que MATLAB nos da respuestas en notacin cientfica.Corroboramos que >> skew2_r1 = [0 -r1(3) r1(2);r1(3) 0 -r1(1);-r1(2) r1(1) 0]
Skew2_r1 =
0 -0.7890 -0.4360 0.7890 0 -0.4330 0.4360 0.4330 0
>> (skew2_r1*r2')'
ans =
-0.5002 0.6121 0.6128
>> r3
r3 =
-0.5000 0.6120 0.6120
REPRESENTACIONES DE LA MATRIZ DE ROTACINa) Usando el mtodo simblico de MATLAB verifique la frmula para:1. Representacin por ngulos de Euler ZYZ2. Representacin por ngulos de Euler RPY>> syms phy th xhy>> rotz(phy)*roty(th)*rotz(xhy) %Angulos de Euler ZYZ ans = [ cos(phy)*cos(th)*cos(xhy) - sin(phy)*sin(xhy), - cos(xhy)*sin(phy) - cos(phy)*cos(th)*sin(xhy), cos(phy)*sin(th)][ cos(phy)*sin(xhy) + cos(th)*cos(xhy)*sin(phy), cos(phy)*cos(xhy) - cos(th)*sin(phy)*sin(xhy), sin(phy)*sin(th)][ -cos(xhy)*sin(th), sin(th)*sin(xhy), cos(th)]
>> rotz(phy)*roty(th)*rotx(xhy) %Angulos de Euler RPY ans = [ cos(phy)*cos(th), cos(phy)*sin(th)*sin(xhy) - cos(xhy)*sin(phy), sin(phy)*sin(xhy) + cos(phy)*cos(xhy)*sin(th)][ cos(th)*sin(phy), cos(phy)*cos(xhy) + sin(phy)*sin(th)*sin(xhy), cos(xhy)*sin(phy)*sin(th) - cos(phy)*sin(xhy)][ -sin(th), cos(th)*sin(xhy), cos(th)*cos(xhy)]b) Para la matriz de rotacin del apartado 4, halle los ngulos de Euler ZYZ y RPYHALLAMOS LOS NGULOS DE EULER ZYZPara :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> phy = atan2(R1(2,3),R1(1,3))
phy =
0.2907
>> th = atan2(sqrt((R1(1,3))^2+(R1(2,3))^2),R1(3,3))
th =
0.6488
>> phy = atan2(R1(2,3),R1(1,3))
phy =
0.2907
Comparamos los ngulos encontrados con los que se obtienen usando la funcin tr2eul():>> [phy th xhy]
ans =
0.2907 0.6488 -0.2350
>> tr2eul(R1)
ans =
0.2907 0.6488 -0.2350Aplicamos la funcin eul2tr() para comprobar que obtenemos la matriz de rotacin original:>> t2r(eul2tr(phy,th,xhy))
ans =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968Nota: Usamos adicionalmente la funcin t2r() para extraer la sub-matriz de rotacin de la matriz de transformacin homogeneaPara :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> phy = atan2(R2(2,3),R2(1,3))
phy =
-0.0595
>> th = atan2(sqrt((R2(1,3))^2+(R2(2,3))^2),R2(3,3))
th =
0.9119
>> xhy = atan2(R2(3,2),-R2(3,1))
xhy =
0.8858
Comparamos los ngulos encontrados con los que se obtienen usando la funcin tr2eul():>> [phy th xhy]
ans =
-0.0595 0.9119 0.8858
>> tr2eul(R2)
ans =
-0.0595 0.9119 0.8856Aplicamos la funcin eul2tr() para comprobar que obtenemos la matriz de rotacin original:>> t2r(eul2tr(phy,th,xhy))
ans =
0.4327 -0.4356 0.7893 0.7500 0.6598 -0.0470 -0.5003 0.6123 0.6122
>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120Nota: Usamos adicionalmente la funcin t2r() para extraer la sub-matriz de rotacin de la matriz de transformacin homogneaHALLAMOS LOS NGULOS DE EULER RPYPara :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> phy = atan2(R1(2,1),R1(1,1))
phy =
-0.0012
>> th = atan2(-R1(3,1),sqrt((R1(3,2))^2+(R1(3,3))^2))
th =
0.6281
>> xhy = atan2(R1(3,2),R1(3,3))
xhy =
-0.1748Comparamos los ngulos encontrados con los que se obtienen usando la funcin tr2rpy():>> [phy th xhy]
ans =
-0.0012 0.6281 -0.1748
>> tr2rpy(R1,'zyx')
ans =
-0.0012 0.6281 -0.1748Nota: Hay que agregar la opcin zxy como opcin a la funcin tr2rpy() para que nos devuelva los ngulos en el orden ZYXPara :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> phy = atan2(R2(2,1),R2(1,1))
phy =
1.0472
>> th = atan2(-R2(3,1),sqrt((R2(3,2))^2+(R2(3,3))^2))
th =
0.5239
>> xhy = atan2(R2(3,2),R2(3,3))
xhy =
0.7854Comparamos los ngulos encontrados con los que se obtienen usando la funcin tr2rpy():>> [phy th xhy]
ans =
1.0472 0.5239 0.7854
>> tr2rpy(R2,'zyx')
ans =
1.0472 0.5236 0.7848Nota: Hay que agregar la opcin zxy como opcin a la funcin tr2rpy() para que nos devuelva los ngulos en el orden ZYX
c) Para la matriz de rotacin del apartado 4 halle el ngulo y eje segn las expresiones de la teora y compruebe el resultado con la instruccin rotvec(v,th)Para :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> theta1 = acos((trace(R1)-1)/2)
theta1 =
0.6511
>> k1 = (1/(2*sin(theta1)))*[R1(3,2)-R1(2,3);R1(1,3)-R1(3,1);R1(2,1)-R1(1,2)]
k1 =
-0.2590 0.9624 0.0826
>> angvec2r(theta1,k1')
ans =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
Para :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> theta2 = acos((trace(R2)-1)/2)
theta2 =
1.2106
>> k2 = (1/(2*sin(theta2)))*[R2(3,2)-R2(2,3);R2(1,3)-R2(3,1);R2(2,1)-R2(1,2)]
k2 =
0.3521 0.6887 0.6337
>> angvec2r(theta2,k2')
ans =
0.4328 -0.4360 0.7890 0.7500 0.6596 -0.0469 -0.5000 0.6121 0.6125En la ltima versin de la toolbox de robtica se usa angvec2r() en lugar de rotvec()d) Suponer que R es generado por una rotacin de 90 en Z, luego una rotacin de 30 en Y, luego una rotacin de 60 en X (todos en el sistema mvil), hallar su ngulo y eje.>> R = rotz(degtorad(90))*roty(degtorad(30))*rotx(degtorad(60))
R =
0.0000 -0.5000 0.8660 0.8660 0.4330 0.2500 -0.5000 0.7500 0.4330
>> theta = acos((trace(R)-1)/2)
theta =
1.6378
>> k = (1/(2*sin(theta)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]
k =
0.2506 0.6846 0.6846e) Para el ngulo y eje obtenido en c), halle R utilizando la formula de RodriguezPara :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> theta1
theta1 =
0.6511
>> k1
k1 =
-0.2590 0.9624 0.0826
>> ss1 = [0 -k1(3) k1(2);k1(3) 0 -k1(1);-k1(2) k1(1) 0]
Ss1 =
0 -0.0826 0.9624 0.0826 0 0.2590 -0.9624 -0.2590 0
>> R1_ = eye(3)+sin(theta1)*skew1+(1-cos(theta1))*skew1^2
R1_ =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
Para :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> theta2
theta2 =
1.2106
>> k2
k2 =
0.3521 0.6887 0.6337
>> skew2 = [0 -k2(3) k2(2);k2(3) 0 -k2(1);-k2(2) k2(1) 0]
skew2 =
0 -0.6337 0.6887 0.6337 0 -0.3521 -0.6887 0.3521 0
>> R2_ = eye(3)+sin(theta2)*skew2+(1-cos(theta2))*skew2^2
R2_ =
0.4329 -0.4360 0.7890 0.7500 0.6597 -0.0469 -0.5000 0.6121 0.6126
f) Para la R del apartado c) y d) obtenga la representacin de R segn la representacin de RodrguezPara :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> theta1
theta1 =
0.6511
>> k1
k1 =
-0.2590 0.9624 0.0826
>> R1_ = theta1*k1
R1_ =
-0.1686 0.6266 0.0537Para :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> theta2
theta2 =
1.2106
>> k2
k2 =
0.3521 0.6887 0.6337
>> R2_ = theta2*k2
R2_ =
0.4262 0.8337 0.7671
Para :>> R
R =
0.0000 -0.5000 0.8660 0.8660 0.4330 0.2500 -0.5000 0.7500 0.4330
>> theta
theta =
1.6378
>> k
k =
0.2506 0.6846 0.6846
>> R_ = theta*k
R_ =
0.4104 1.1212 1.1212REPRESENTACIN POR CUATERNIOSa) Para la matriz de rotacin del apartado 5c) halle su representacin en cuaternios usando las siguientes instrucciones: quaternion([s v1 v2 v3]), quaternion(v,th), quaternion(R)Para :>> R1
R1 =
0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
>> q1 = [cos(theta1/2) sin(theta1/2)*k1']
q1 =
0.9475 -0.0828 0.3078 0.0264
>> q1 = Quaternion(theta1,k1) q1 = 0.94748 < -0.082824, 0.30779, 0.026403 > >> q1 = Quaternion(R1) q1 = 0.94748 < -0.082824, 0.30779, 0.026402 >Para :>> R2
R2 =
0.4330 -0.4360 0.7890 0.7500 0.6600 -0.0470 -0.5000 0.6120 0.6120
>> q2 = [cos(theta2/2) sin(theta2/2)*k2']
q2 =
0.8223 0.2003 0.3919 0.3606
>> q2 = Quaternion(theta2,k2) q2 = 0.82234 < 0.20036, 0.3919, 0.36058 > >> q2 = Quaternion(R2) q2 = 0.82234 < 0.20032, 0.392, 0.3605 >b) El punto P1(1,1,0) es rotado alrededor del eje Y en 90, hallar su nueva posicin en el sistema de referencia segn:1. Mtodo convencional>> P1 = [1 1 0];>> R = fix(roty(degtorad(90)))
R =
0 0 1 0 1 0 -1 0 0
>> P1_ = (R*P1')'
P1_ =
0 1 -12. Usando cuaternios>> P1 = [1 1 0];>> P1q = Quaternion([0 P1]) P1q = 0 < 1, 1, 0 >>> R = roty(degtorad(90))
R =
0.0000 0 1.0000 0 1.0000 0 -1.0000 0 0.0000
>> theta = acos((trace(R)-1)/2)
theta =
1.5708>> k = (1/(2*sin(theta)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]
k =
0 1 0
>> q = Quaternion([cos(theta/2) sin(theta/2)*k']) q = 0.70711 < 0, 0.70711, 0 >
>> qq = Quaternion([q.s (-1)*q.v]) qq = 0.70711 < 0, -0.70711, 0 >
>> P1_ = mtimes(mtimes(q,P1q),qq) P1_ = 0 < 2.2204e-16, 1, -1 > >> P1_ = fix(P1_.v)
P1_ =
0 1 -1
FUNCIONES (ARCHIVOS .M)a) Realizar una funcin (archivo .m) que convierta de la matriz de rotacin R a representacin ngulo y eje.%rot2angeje Obtiene el angulo y eje de giro a partir de una matriz de% rotacion%% +----------+% | SINTAXIS |% +----------+%% [ang,eje] = rot2angeje(R,opciones)%% Donde:% R es la matriz de rotacion, de orden 3x3%% Opciones:% 'deg': Devuelve ang en angulos sexagesimales (radianes por defecto) function [ang,eje] = rot2angeje(R,varargin) opt.deg = false; opt = tb_optparse(opt, varargin); %Verificamos que la matriz de rotacion sea de orden 3x3 d = size(R); if (d(1) ~= d(2) || d(1) ~= 3) error('La matriz debe ser de orden 3x3'); end ang = acos((trace(R)-1)/2); eje = (1/(2*sin(ang)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]; if opt.deg ang = ang * 180/pi; endend
b) Realizar una funcin (archivo .m) que convierta de la representacin de R por frmula de Rodrguez a R.%RODRIG2R Convierte de la representacion de R por frmula de Rodriguez a R%% +----------+% | SINTAXIS |% +----------+%% R = rodrig2R(Rr)%% Donde:% R es la matriz de rotacion resultante% Rr es el vector fila o columna de 3 elementos que representa R% segn la frmula de Rodriguez (theta*k) function R = rodrig2R(Rr) %Verificamos el orden y tamao del vector fr Rodriguez if numel(Rr) ~= 3 error('El vector de Rodriguez debe ser de orden 3x1 1x3'); end theta = sqrt(dot(Rr,Rr)); k = Rr./theta; skew = [0 -k(3) k(2);k(3) 0 -k(1);-k(2) k(1) 0]; R = eye(3) + sin(theta)*skew + (1-cos(theta))*skew^2;end
c) Hacer una funcin que multiplique dos cuaternios y %MULCUAT Realiza el producto de 2 cuaternios%% +----------+% | SINTAXIS |% +----------+%% M = mulcuat(q1,q2)%% Donde:%% q1 y q2 son cuaterions de orden 4x1 1x4% M es el cuaternio resultante de orden 1x4 function M = mulcuat(q1,q2) %Verificamos el numero de elementos de los vectores if ~all([numel(q1) numel(q2)] == 4) error('Los cuaternios se componen de 4 elementos'); end d1 = size(q1); d2 = size(q2); %Hallamos la transpuesta de los vectores en el caso que el orden sea de %4x1 para q1 para q2 if d1(1) == 4 q1 = q1'; end if d2(1) == 4 q2 = q2'; end %Hallamos la parte escalar y vetorial de q1 y q2 s1 = q1(1); s2 = q2(1); v1 = q1(2:4); v2 = q2(2:4); M = [s1*s2-dot(v1,v2) cross(v1,v2)+s1*v2+s2*v1];end
d) Realizar una funcin que transforme de la matriz de rotacin a cuaternios%ROT2CUAT Evalua una matriz de rotacion y halla su representacion% mediante cuaternios%% +----------+% | SINTAXIS |% +----------+%% Q = rot2cuat(R)%% Donde:%% R es la matriz de rotacion evaluada, de orden 3x3% Q es el cuaternio resultante, de orden 1x4 function Q = rot2cuat(R) %Verificamos que la matriz de rotacion sea de orden 3x3 d = size(R); if (d(1) ~= d(2) || d(1) ~= 3) error('La matriz debe ser de orden 3x3'); end theta = acos((trace(R)-1)/2); k = (1/(2*sin(theta)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]; Q = [cos(theta/2) sin(theta/2)*k'];end