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
