UNIVERSIDAD NACIONAL DE SAN AGUSTÍN

ESCUELA PROFESIONAL DE INGENIERÍA ELECTRÓNICA

FACULTAD DE INGENIERÍA DE PRODUCCIÓN Y SERVICIOS

Curso:
Mecanismos y Robótica

Grupo: B

Docente:
Ing. Edwin Javier Villalba Cabrera

Integrantes:
Mamani Aqquepucho, Rodrigo Alvaro
Palo Soto, Nair Edson
LINK DEL VIDEO.
https://drive.google.com/file/d/1_67WlDPDl_qv5I_-PiZFUv4WjqBUIa0v/view?usp=sharing
Tema: LAB Nº1

Arequipa - Perú
2022-A

1
PRÁCTICA Nº 1 de ROBÓTICA

TRANSFORMACIONES ESPACIALES y
MATRIZ DE ROTACIÓN
Autor: Lizardo Pari.

I .- OBJETIVOS:
• Visualizar en 3D sistemas de coordenadas con diferentes posiciones y
orientaciones.
• Comprobación matemática de la nueva posición de un punto al someterlo a
una transformación.
• Verificar las propiedades de la matriz de rotación
• Comprobar las transformaciones de la matriz de rotación a par de rotación,
fórmula de Rodríguez, ángulos de Euler y cuaternios.

II.- CONOCIMIENTOS PREVIOS

• ¿Qué es una matriz homogénea?, de qué está constituida?
Estas se utilizan para representar la posición y orientación de un sistema girado y
trasladado con respecto a un sistema fijo.

H representa una traslación seguida de una rotación en el sistema resultante. El

vector de traslación queda como un vector 4x1 en el sistema euclídeo tridimensional:

• ¿Cuáles son las principales propiedades de la matriz de rotación?

Su principal propiedad es que son cuadradas y con valores reales, sin embargo, se
pueden definir sobre otros cuerpos .

• ¿Cuáles son las representaciones de la matriz de rotación?

Una matriz de rotación es la matriz que representa una rotación en el espacio
euclídeo.
En tres dimensiones, representan las rotaciones de manera concisa y se usan
frecuentemente en geometría, física e informática.

2
 • ¿Qué es un cuaternios?
Son una generalización de los números complejos de R2 a R4, además, es un
sistema multiplicativo no conmutativo

III.- PROCEDIMIENTO
1.- Introducción
La base matemática de la robótica la constituye la matemática del movimiento del cuerpo rígido. Aquí
se estudia las diversas formas de representar la orientación y posición de un cuerpo (que está
asociado a su propio sistema de referencia) en el espacio tridimensional. Básicamente la orientación y
posición de un cuerpo rígido está determinado por la matriz de rotación y el vector de traslación, éstos
se pueden representar conjuntamente por medio de una matriz homogénea. Una matriz de rotación
es una matriz ortogonal con propiedades particulares que la hacen especial, ésta a la vez posee
diversas representaciones descritas en la literatura que van desde el par de rotación, la fórmula de
Rodríguez, los ángulos de Euler y la representación por cuaternios. En la presente práctica se
ejercitará con todos ellos, utilizando el soporte de Matlab y el toolbox de robótica escrito por Peter
Corke.

2.- Representación de la rotación y traslación.

a). Representa una rotación y traslación nula.

CÓDIGO
>> clear
>> R=eye(3)
t=[0 0 0]'
T0=[R t; 0 0 0 1];

R =

1 0 0
0 1 0
0 0 1

t =

3
 0
0
0

>>
>> trplot(T0,'frame','T0','color','b')

b). Representar una rotación de 90 grados en el eje Y

T1=roty(pi/2) ó
T1=troty(pi/2)
hold on

>> T1=roty(pi/2*180/pi)

T1 =

0 0 1
0 1 0
-1 0 0

>> hold on
>> trplot(T1,'frame','T1','color','r');

4
c). Representar una rotación de -90 grados en el eje Z
T2=rotz(-pi/2) ó T2=trotz(-pi/2)
trplot(T2,’frame’,’T2’,’color’,’g’);

>> T2=rotz(-pi/2*180/pi)

T2 =

0 0 -1
0 1 0
1 0 0

>> hold on
>> trplot(T2,'frame','T2','color','g');

5
d). Representar una traslación de 0.5 en el eje X
T3=transl(0.5,0,0);
trplot(T3,’frame’,’T3’,’color’,’m’);

>> T3=transl(0.5,0,0)

T3 =

1.0000 0 0 0.5000
0 1.0000 0 0
0 0 1.0000 0
0 0 0 1.0000

>> T3=transl(0.5,0,0)
trplot(T3,’frame’,’T3’,’color’,’m’);

6
e). Representa una traslación de 0.5 en el eje X, luego una rotación de -90
grados en el eje Z (en el sistema resultante).

T4=T3*T2
trplot(T4,’frame’,’T4’,
’color’,’c’);
>>
T2=trotz(-pi/2*180/pi)

T2 =

0 1 0
0
-1 0 0
0
0 0 1
0
0 0 0
1

>> T4=T3*T2

T4 =

0 1.0000
0 0.5000
-1.0000 0
0 0
0 0
1.0000 0
0 0
0 1.0000

7
>>
trplot(T4,'frame','T4',
'color','c');

f). Representa una traslación de 0.5 en el eje X, luego una rotación de -90
grados en el eje Z, y posteriormente una rotación de 90 en el eje Y (todos en el
sistema resultante).

T5=T4*T1
trplot(T5,’frame’,’T5’,’color’,’y’);

>> T1=troty(pi/2*180/pi)

T1 =

0 0 1 0
0 1 0 0
-1 0 0 0
0 0 0 1

>> T5=T4*T1

T5 =

0 1.0000 0 0.5000
0 0 -1.0000 0
-1.0000 0 0 0
0 0 0 1.0000

>> trplot(T5,'frame','T5','color','y');

8
g) Representar dos matrices homogéneas que describan:
1.- traslación de (0.5,0.5,0) luego rotación en el eje X en 30 grados (sist.
resultante).
2.- traslación de (-0.5,0.5,-0.5) luego rotación en el eje Z en 45 grados (sist.
result.).

NOTA1: probar con “tr2rt()”

NOTA2: probar “tranimate()”
>> T6=transl(0.5,0.5,0)*trotx(pi/6*180/pi)

T6 =

1.0000 0 0 0.5000
0 0.8660 -0.5000 0.5000
0 0.5000 0.8660 0
0 0 0 1.0000

>> T7=transl(-0.5,0.5,-0.5)*trotz(pi/4*180/pi)

T7 =

0.7071 -0.7071 0 -0.5000

0.7071 0.7071 0 0.5000
0 0 1.0000 -0.5000
0 0 0 1.0000

9
>> figure
>> T=eye(4)

T=

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

>> trplot(T,'frame','T0','color','b')
hold on
>> trplot(T6,'frame','T6','color','r')
>> trplot(T7,'frame','T7','color','g')

3.- Operaciones con transformaciones.

a). ¿Cuáles son las nuevas coordenadas de p1=(2,1,3) si éste pertenece a un
cuerpo rígido que ha sufrido una rotación de 90 grados en el eje Y?.
>> p1=[2 1 3]'

p1 =

2
1
3

>> R=roty(pi/2*180/pi)

R =

0 0 1
0 1 0
-1 0 0

>> t=[0 0 0]'

t =

0
0

10
 0

>> p1p=R*p1+t

p1p =

3
1
-2

b) ¿cuales son las nuevas coordenadas de p1=(2,1,3) si éste ha sufrido una

traslación de 2 en eje X?

>> I=eye(3)

I =

1 0 0
0 1 0
0 0 1

>> t=[2 0 0]'

t =

2
0
0

>> p2p=I*p1+t

p2p =

4
1
3

c) ¿Cuáles son las nuevas coordenadas de p1=(2,1,3) si éste ha sufrido una

traslación de 2 en eje X y luego una rotación de 90 grados en el eje Y?
>> p3p=R*p1+t

p3p =

5
1
-2
>> T1=transl(2,0,0)

T1 =

1 0 0 2
0 1 0 0
0 0 1 0
0 0 0 1

>> T2=troty(pi/2*180/pi)

T2 =

0 0 1 0
0 1 0 0
-1 0 0 0

11
 0 0 0 1

>> T=T1*T2

T =

0 0 1 2
0 1 0 0
-1 0 0 0
0 0 0 1

>> p3p_=T*[p1;1]

p3p_ =

5
1
-2
1

d) ¿Cuales son las nuevas coordenadas de p2=(10,7,6) si éste ha sufrido

una traslación de (10,5,0), luego una rotación de -90 grados en el eje Y luego
una rotación de 90 grados en el eje X
>> p2=[10 7 6]'

p2 =

10
7
6
>> T1=transl(10,5,0)

T1 =

1 0 0 10
0 1 0 5
0 0 1 0
0 0 0 1

>> T2=troty(-pi/2*180/pi)

T2 =

0 0 -1 0
0 1 0 0
1 0 0 0
0 0 0 1

>> T3=trotx(pi/2*180/pi)

T3 =

1 0 0 0
0 0 -1 0
0 1 0 0
0 0 0 1

>> T=T1*T2*T3

T =

0 -1 0 10
0 0 -1 5
1 0 0 0
0 0 0 1

12
4.- Propiedades de la matriz de rotación.
a). Consecuencia del movimiento de un robot de 3gdl se consiguió las siguientes
matrices de rotación:

 0.809139 -0.101016 0.57887  0.4330 -0.4356 0.7891

   
R1 = -0.0009527 0.984886 0.173199  R2 = 0.7500 0.6597 -0.0474 
 -0.587616 -0.140694 0.796814 -0.500 0.6124 0.6124 

Para cada una de estas matrices,

• Halle su determinante
• Verifique R-1=RT
• Si se considera sus columnas (o filas) de R como r1, r2, y r3, verifique que r r1 1T
=r r2 2T =r r3 3T =1 r r1 T1 =r r2 T2 =r r3 T3 =1 (1) r r1 2T =r r1 3T =r r2 3T =0 r r1 T2 =r r1 T3
=r r2 T3 =0

[r r1]x 2 =r3 [r r1]x 2 =r3

[r2]x r3 =r1 ó [r2]x r3 =r1
[r3]x r1 =r2 [r3]x r1 =r2

Figura 1.- traslación (1,0.5,0)

*PARA R1
>> 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
#DETERMINANTE
>> det(R1)

ans =

1.0000
#COMPROBAR LA INVERSA CON LA MATRIZ TRANSPUESTA
>> inv(R1)

13
ans =

0.8091 -0.0010 -0.5876

-0.1010 0.9849 -0.1407
0.5789 0.1732 0.7968

>> R1'

ans =

0.8091 -0.0010 -0.5876

-0.1010 0.9849 -0.1407
0.5789 0.1732 0.7968
#Si se considera sus columnas (o filas) de R como r1, r2, y r3, verifique
que r r1 1T =r r2 2T =r r3 3T =1 r r1 T1 =r r2 T2 =r r3 T3 =1 (1) r r1 2T
=r r1 3T =r r2 3T =0 r r1 T2 =r r1 T3 =r r2 T3 =0

>> r1=R1(:,1)

r1 =

0.8091
-0.0010
-0.5876

>> r2=R1(:,2)

r2 =

-0.1010
0.9849
-0.1407

>> r3=R1(:,3)

r3 =

0.5789
0.1732
0.7968

>> R1

R1 =

0.8091 -0.1010 0.5789

-0.0010 0.9849 0.1732
-0.5876 -0.1407 0.7968

>> r1'*r1

ans =

1.0000

>> r2'*r2

ans =

1.0000

>> r3'*r3

14
ans =

1.0000

>> r1'*r2

ans =

-2.4061e-07

>> r1'*r3

ans =

6.3082e-07

>> r2'*r3

ans =

-8.1052e-07

>> S1=skew(r1)

S1 =

0 0.5876 -0.0010
-0.5876 0 -0.8091
0.0010 0.8091 0

>> S2=skew(r2)

S2 =

0 0.1407 0.9849
-0.1407 0 0.1010
-0.9849 -0.1010 0

>> S3=skew(r3)

S3 =

0 -0.7968 0.1732
0.7968 0 -0.5789
-0.1732 0.5789 0

>> S1*r2

ans =

0.5789
0.1732
0.7968

>> r3

r3 =

0.5789
0.1732
0.7968

>> S2*r3

15
ans =

0.8091
-0.0010
-0.5876

>> r1

r1 =

0.8091
-0.0010
-0.5876

>> S3*r1

ans =

-0.1010
0.9849
-0.1407

>> r2

r2 =

-0.1010
0.9849
-0.1407

>> 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.7968

>> R1

R1 =

0.8091 -0.1010 0.5789

-0.0010 0.9849 0.1732
-0.5876 -0.1407 0.7968

>> r1*r1'

ans =

1.0000

>> r2*r2'

16
ans =

1.0000

>> r3*r3'

ans =

1.0000

>> r1*r2'

ans =

-4.0577e-07

>> r1*r3'

ans =

1.0427e-06

>> r2*r3'

ans =

-3.4113e-07

>> S1=skew(r1)

S1 =

0 -0.5789 -0.1010
0.5789 0 -0.8091
0.1010 0.8091 0

>> S2=skew(r2)

S2 =

0 -0.1732 0.9849
0.1732 0 0.0010
-0.9849 -0.0010 0

>> S3=skew(r3)

S3 =

0 -0.7968 -0.1407
0.7968 0 0.5876
0.1407 -0.5876 0

>> S1*r2'

ans =

-0.5876
-0.1407
0.7968

>> r3

r3 =

17
 -0.5876 -0.1407 0.7968

>> S2*r3'

ans =

0.8091
-0.1010
0.5789

>> r1

r1 =

0.8091 -0.1010 0.5789

>> S3*r1'

ans =

-0.0010
0.9849
0.1732

>> r2

r2 =

-0.0010 0.9849 0.1732

*PARA R2

>> R2=rotz(pi/3*180/pi)*roty(pi/6*180/pi)*rotx(pi/4*180/pi)

R2 =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

#DETERMINANTE DE R2
>> det(R2)

ans =

1.0000

#COMPROBAR LA INVERSA CON LA MATRIZ TRANSPUESTA

>> inv(R2)

ans =

0.4330 0.7500 -0.5000

-0.4356 0.6597 0.6124
0.7891 -0.0474 0.6124

>> R2'

ans =

0.4330 0.7500 -0.5000

-0.4356 0.6597 0.6124

18
 0.7891 -0.0474 0.6124

#Si se considera sus columnas (o filas) de R como r1, r2, y r3, verifique
que r r1 1T =r r2 2T =r r3 3T =1 r r1 T1 =r r2 T2 =r r3 T3 =1 (1) r r1 2T
=r r1 3T =r r2 3T =0 r r1 T2 =r r1 T3 =r r2 T3 =0

>> r1=R2(:,1)

r1 =

0.4330
0.7500
-0.5000

>> r2=R2(:,2)

r2 =

-0.4356
0.6597
0.6124

>> r3=R2(:,3)

r3 =

0.7891
-0.0474
0.6124

>> R2

R2 =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> r1'*r1

ans =

>> r2'*r2

ans =

1.0000

>> r3'*r3

ans =

>> r1'*r2

ans =

-2.7756e-17

>> r1'*r3

19
ans =

5.5511e-17

>> r2'*r3

ans =

-5.5511e-17

>> S1=skew(r1)

S1 =

0 0.5000 0.7500
-0.5000 0 -0.4330
-0.7500 0.4330 0

>> S2=skew(r2)

S2 =

0 -0.6124 0.6597
0.6124 0 0.4356
-0.6597 -0.4356 0

>> S3=skew(r3)

S3 =

0 -0.6124 -0.0474
0.6124 0 -0.7891
0.0474 0.7891 0

>> S1*r2

ans =

0.7891
-0.0474
0.6124

>> r3

r3 =

0.7891
-0.0474
0.6124

>> S2*r3

ans =

0.4330
0.7500
-0.5000

>> r1

r1 =

0.4330
0.7500

20
 -0.5000

>> S3*r1

ans =

-0.4356
0.6597
0.6124

>> r2

r2 =

-0.4356
0.6597
0.6124

>> r1=R2(1,:)

r1 =

0.4330 -0.4356 0.7891

>> r2=R2(2,:)

r2 =

0.7500 0.6597 -0.0474

>> r3=R2(3,:)

r3 =

-0.5000 0.6124 0.6124

>> R2

R2 =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> r1*r1'

ans =

1.0000

>> r2*r2'

ans =

>> r3*r3'

ans =

>> r1*r2'

21
ans =

>> r1*r3'

ans =

-5.5511e-17

>> r2*r3'

ans =

5.5511e-17

>> S1=skew(r1)

S1 =

0 -0.7891 -0.4356
0.7891 0 -0.4330
0.4356 0.4330 0

>> S2=skew(r2)

S2 =

0 0.0474 0.6597
-0.0474 0 -0.7500
-0.6597 0.7500 0

>> S3=skew(r3)

S3 =

0 -0.6124 0.6124
0.6124 0 0.5000
-0.6124 -0.5000 0

>> S1*r2'

ans =

-0.5000
0.6124
0.6124

>> r3

r3 =

-0.5000 0.6124 0.6124

>> S2*r3'

ans =

0.4330
-0.4356
0.7891

>> r1

22
r1 =

0.4330 -0.4356 0.7891

>> S3*r1'

ans =

0.7500
0.6597
-0.0474

>> r2

r2 =

0.7500 0.6597 -0.0474

5.- Representaciones de la matriz de rotación.

a) Usando el método simbólico de Matlab verifique la fórmula para:
1.- Representación por ángulos de Euler ZYZ.
2.- Representación por ángulos de Euler RPY. NOTA: usar syms phy th xhy.
>> 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),os(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 rotación del apartado 4, halle los ángulos de Euler ZYZ y RPY (en
grados). NOTA: use tr2eul() y tr2rpy(). Además: rpy2tr() y eul2tr().

PARA R2
>> R2=rotz(pi/3*180/pi)*roty(pi/6*180/pi)*rotx(pi/4*180/pi)

R2 =

23
 0.4330 -0.4356 0.7891
0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> tr2eul(R2)*180/pi

ans =

-3.4349 52.2388 50.7685

>> tr2rpy(R2, 'zyx')*180/pi

ans =

45.0000 30.0000 60.0000

c) Para las matrices de rotación del apartado 4 halle el ángulo y eje según las expresiones
de la teoría y compruebe el resultado con la instrucción [ang vec] = tr2angvec(R).
>> theta2 = acos((trace(R2)-1)/2)

theta2 =

1.2105

>> 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.3525
0.6888
0.6335

>> [ang vec] = tr2angvec(R2)

ang =

1.2105

vec =

0.3525 0.6888 0.6335

d) Suponer que R2 es generado por una rotación de 90 grados en Z, luego una rotación de
30º en Y, luego una rotación de 60º en X (todos en el sistema móvil), hallar su ángulo y
eje, luego pruebe con tr2rpy(R2,’zyx’,’deg’) e interpretar.

>> R3=rotz(pi/2*180/pi)*roty(pi/6*180/pi)*rotx(pi/3*180/pi)

R3 =

0 -0.5000 0.8660

24
 0.8660 0.4330 0.2500
-0.5000 0.7500 0.4330

>> theta3 = acos((trace(R3)-1)/2)

theta3 =

1.6378

>> k3 = (1/(2*sin(theta3)))*[R3(3,2)-R3(2,3);R3(1,3)-R3(3,1);R3(2,1)-R3(1,2)]

k3 =

0.2506
0.6846
0.6846

>> tr2rpy(R3, 'zyx', 'deg')

ans =

60.0000 30.0000 90.0000

e) Para el ángulo y eje obtenido en c), halle la rotación R utilizando la fórmula de

Rodríguez.

>> R2=rotz(pi/4*180/pi)*roty(pi/3*180/pi)*rotx(pi/6*180/pi)

R2 =

0.3536 -0.3062 0.8839

0.3536 0.9186 0.1768
-0.8660 0.2500 0.4330

>> theta2 = acos((trace(R2)-1)/2)

theta2 =

1.2105

>> 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.0391
0.9350
0.3525

>> R3=rotz(pi/2*180/pi)*roty(pi/6*180/pi)*rotx(pi/3*180/pi)

R3 =

0 -0.5000 0.8660
0.8660 0.4330 0.2500
-0.5000 0.7500 0.4330

>> theta3 = acos((trace(R3)-1)/2)

theta3 =

25
 1.6378

>> k3 =
(1/(2*sin(theta3)))*[R3(3,2)-R3(2,3);R3(1,3)-R3(3,1);R3(2,1)-R3(1,2)]

k3 =

0.2506
0.6846
0.6846

>> rod2 = theta2*k2

rod2 =

0.0474
1.1318
0.4267

>> rod3 = theta3*k3

rod3 =

0.4104
1.1212
1.1212

f) Para las rotaciones del apartado c) y d) obtenga su representación de

Rodríguez (nota: es un vector).

>> R2=rotz(pi/4*180/pi)*roty(pi/3*180/pi)*rotx(pi/6*180/pi)

R2 =

0.3536 -0.3062 0.8839

0.3536 0.9186 0.1768
-0.8660 0.2500 0.4330

>> theta2 = acos((trace(R2)-1)/2)

theta2 =

1.2105

>> 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.0391
0.9350
0.3525

>> theta2 = acos((trace(R2)-1)/2)

theta2 =

1.2105

>> k2 = (1/(2*sin(theta2)))*[R2(3,2)-R2(2,3);R2(1,3)-R2(3,1);R2(2,1)-R2(1,2)]

26
k2 =

0.0391
0.9350
0.3525

>> R3=rotz(pi/2)*roty(pi/6)*rotx(pi/3)

R3 =

0.9996 -0.0272 0.0096

0.0274 0.9995 -0.0180
-0.0091 0.0183 0.9998

>> theta3 = acos((trace(R3)-1)/2)

theta3 =

0.0341

>> k3 = (1/(2*sin(theta3)))*[R3(3,2)-R3(2,3);R3(1,3)-R3(3,1);R3(2,1)-R3(1,2)]

k3 =

0.5319
0.2751
0.8009

>> rod2 = theta2*k2

rod2 =

0.0474
1.1318
0.4267

>> rod3 = theta3*k3

rod3 =

0.0182
0.0094
0.0273

g) Hallar [V D]=eig(R) e interpretar su resultado. NOTA: usar: angle(D(1,1)) ó

angle(D(2,2)).
>> R2=rotz(pi/3*180/pi)*roty(pi/6*180/pi)*rotx(pi/4*180/pi)

R2 =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> [V D]=eig(R2)

V =

27
 0.6617 + 0.0000i 0.6617 + 0.0000i 0.3525 + 0.0000i
-0.1835 - 0.4787i -0.1835 + 0.4787i 0.6888 + 0.0000i
-0.1687 + 0.5205i -0.1687 - 0.5205i 0.6335 + 0.0000i

D =

0.3526 + 0.9358i 0.0000 + 0.0000i 0.0000 + 0.0000i

0.0000 + 0.0000i 0.3526 - 0.9358i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i

>> k_ = V(:,3) %Eje de rotación

k_ =

0.3525
0.6888
0.6335

>> th_ = angle(D(1, 1)) %Ángulo

th_ =

1.2105

6.- Matrices homogéneas.

a) Se tiene t=[2 1 -1]T y la rotación R1 del apartado 4, formar la matriz
homogénea H para:
1.- Rotación seguida de una traslación.
>> t=[2 1 -1]'

t =

2
1
-1

>> R=rotz(pi/3*180/pi)*roty(pi/6*180/pi)*rotx(pi/4*180/pi)

R =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

2.- Traslación seguida de una rotación.

>> t_=transl(t)

t_ =

1 0 0 2

28
 0 1 0 1
0 0 1 -1
0 0 0 1

NOTA: probar con r2t(), t2r(), rt2tr(), tr2rt().

>> R_=r2t(R)

R_ =

0.4330 -0.4356 0.7891

0
0.7500 0.6597 -0.0474
0
-0.5000 0.6124 0.6124
0
0 0 0
1.0000

>> R_*t_

ans =

0.4330 -0.4356 0.7891

-0.3587
0.7500 0.6597 -0.0474
2.2071
-0.5000 0.6124 0.6124
-1.0000
0 0 0
1.0000

>> t_*R_

ans =

0.4330 -0.4356 0.7891

2.0000
0.7500 0.6597 -0.0474
1.0000
-0.5000 0.6124 0.6124
-1.0000
0 0 0
1.0000

>> t2r(ans)

ans =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> H=rt2tr(R,t)

H =

29
 0.4330 -0.4356 0.7891
2.0000
0.7500 0.6597 -0.0474
1.0000
-0.5000 0.6124 0.6124
-1.0000
0 0 0
1.0000

>> [a b]=tr2rt(H)

a =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

b =

2
1
-1

b) hallar H-1 según:

1.- matemáticamente.
>> inv(H)

ans =

0.4330 0.7500 -0.5000 -2.1160

-0.4356 0.6597 0.6124 0.8238
0.7891 -0.0474 0.6124 -0.9186
0 0 0 1.0000

2.- usando la fórmula de la teoría.

>> [R' -R'*t;0 0 0 1]

ans =

0.4330 0.7500 -0.5000 -2.1160

-0.4356 0.6597 0.6124 0.8238
0.7891 -0.0474 0.6124 -0.9186
0 0 0 1.0000

7.-Representación por cuaternios.

30
a) Para las matrices de rotación del apartado 4, halle su representación en
cuaternios usando:
1.- fórmula de la teoría.
>> R

R =

0.4330 -0.4356 0.7891

0.7500 0.6597 -0.0474
-0.5000 0.6124 0.6124

>> th=acos((trace(R)-1)/2)

th =

1.2105

>>
k=(1/(2*sin(th)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]

k =

0.3525
0.6888
0.6335

>> q=[cos(th/2) sin(th/2)*k']

q =

0.8224 0.2006 0.3919 0.3604

2.- Quaternion([s v1 v2 v3]).

>> UnitQuaternion(q)

ans =

0.82236 < 0.20056, 0.3919, 0.36042 >

3.- Quaternion(ang,vec).

>> UnitQuaternion(th,k)

ans =

0.77095 < 0.22451, 0.43869, 0.40346 >

4.- Quaternion(R).

31
>> UnitQuaternion(R)

ans =

0.82236 < 0.20056, 0.3919, 0.36042 >

>> p1=[1 1 0]'

p1 =

1
1
0

>> R=roty(pi/2*180/pi)

R =

0 0 1
0 1 0
-1 0 0

>> p1p=R*p1

p1p =

0
1
-1

b) El punto P1=[1,1,0] es rotado alrededor del eje Y en 90º, hallar su nueva

posición en el sistema de referencia según:
1.- método convencional.
>> p1

p1 =

1
1
0

>> Qp=Quaternion([0 p1'])

Qp =

0 << 1, 1, 0 >>

>> R

R =

32
 0 0 1
0 1 0
-1 0 0

2.- usando cuaternios. NOTA: usar la función Quaternion(). Ver manual para
usar álgebra de cuaternios.
>> Q=UnitQuaternion(R)

Q =

0.70711 < 0, 0.70711, 0 >

>> Qconj=inv(Q)

Qconj =

0.70711 < 0, -0.70711, 0 >

>> Qpp=Q*Qp*Qconj

Qpp =

0 << 2.2204e-16, 1, -1 >>

>> Qpp.v

ans =

0.0000 1.0000 -1.0000

>> p1p

p1p =

0
1
-1

c) Si q es un cuaternio, probar:
1.- q.s
2.- q.v
3.- q.T
4.- q.R 5.- plot(q).

>> Qpp

33
Qpp =

0 << 2.2204e-16, 1, -1 >>

>> Qpp.s

ans =

>> Qpp.v

ans =

0.0000 1.0000 -1.0000

>> Q.R

ans =

0.0000 0 1.0000
0 1.0000 0
-1.0000 0 0.0000

>> plot(Q)

d)Hallar la rotación de R=rotx(30°)*roty(60°), según:

1.- Método convencional.

>> R1=rotx(pi/6*180/pi)

R1 =

1.0000 0 0
0 0.8660 -0.5000
0 0.5000 0.8660

>> R2=roty(pi/3*180/pi)

R2 =

0.5000 0 0.8660
0 1.0000 0
-0.8660 0 0.5000

>> R=R1*R2

R =

34
 0.5000 0 0.8660
0.4330 0.8660 -0.2500
-0.7500 0.5000 0.4330

2.- por cuaternios.

>> Q1=UnitQuaternion(R1)

Q1 =

0.96593 < 0.25882, 0, 0 >

>> Q2=UnitQuaternion(R2)

Q2 =

0.86603 < 0, 0.5, 0 >

>> Q=Q1*Q2

Q =

0.83652 < 0.22414, 0.48296, 0.12941 >

>> UnitQuaternion(R)

ans =

0.83652 < 0.22414, 0.48296, 0.12941 >

e). Hallar el producto del cuaternio hallado en d) por su

inverso.

>> Qinv=inv(Q)

Qinv =

0.83652 < -0.22414, -0.48296, -0.12941 >

>> Q*Qinv

ans =

1 < 0, 0, 0 >

f) Hallar el cuaternio de I3 (rotación nula) NOTA: I3: matriz identidad de orden 3 (en Matlab:
“eye(3)”).

>> I3=eye(3)

35
I3 =

1 0 0
0 1 0
0 0 1

>> UnitQuaternion(I3)

ans =

1 < 0, 0, 0 >

IV INFORME.
a) Implementar una función (archivo m) que convierta de la matriz de rotación R
a representación ángulo y eje.
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;
end
end

b) Implementar una función (archivo m) que convierte la rotación en

representación de Rodríguez a la forma convencional R.

function R = rodrig2R(Rr)
%Verificamos el orden y tamaño 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

36
c) Implementar una función que multiplique dos cuaternios q1 y q2.

function M = mulcuat(q1,q2)

if ~all([numel(q1) numel(q2)] == 4)
error('Los cuaternios se componen de 4 elementos');
end

d1 = size(q1);
d2 = size(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) Implementar una función que transforme de la matriz de rotación convencional

a cuaternios.

function Q = rot2cuat(R)

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

e) Implementar una función que transforme de cuaternio a la forma convencional

R.
Código de la función para implementar la conversión de un cuaternio unitario
a su equivalente matriz de rotación R(3x3) en MATLAB:

function [] = quat2rot (qs,qv1,qv2,qv3)

theta_q=2*acos(qs);
k_q=(1/sin(theta_q/2)).*[qv1 qv2 qv3];
disp(q.R)
end

De nuevo, como tratamos de representar una rotación con un cuaternio,

entonces debe poseer norma unitaria, para ello utilizaremos el cuaternio “q”
del problema 3:

37
 q = 0.5 < 0.5, 0.5, 0.5 >
>> quat2rot (0.5,0.5,0.5,0.5)
La matriz de rotación R es:
0.0000 0.0000 1.0000
1.0000 0.0000 0.0000
0.0000 1.0000 0.0000

Y como el resultado de R representa una rotación esta debe tener,

determinante unitario puesto que debe ser ortonormal:
>> det([0 0 1;1 0 0;0 1 0])
ans=
1

Además de las pruebas realizadas que validen los programas implementados, se

deberá adjuntar en el informe el código fuente de los programas implementados.

CONCLUSIONES :
● Se puede comprobar las rotaciones de planos .
● Se comprobó que la inversas de matriz y la multiplicación de vectores
● Se pudo comprender los distintos comandos que existen para rotar y trasladar
matrices de cualquier orden
● Se entendió los códigos para trabajar con ángulos de Euler, cuaterniones.

38