LACAR

2017
Mini-Curso:
Fundamentos de
Robótica
Ph.D. César Augusto Peña Cortés
Universidad de Pamplona

1
 LACAR 2017
Contenido
•  Introducción
•  Herramientas matemáEcas
•  CinemáEca Directa
•  CinemáEca Inversa

2
Nota: Este material esta diseñado para solo dos horas y media, dada la amplitud de los
temas su profundidad se encuentra notablemente limitada (curso introductorio)
 LACAR 2017
HERRAMIENTAS
MATEMÁTICAS

3
 LACAR 2017
Localización Espacial

4
Representación de la posición
Vector de posición en coordenadas
•  Cartesianas
•  Cilíndricas
•  Esféricas

5
 Ejercicio:
Obtener las componentes de los vectores U y V sobre los ejes
coordenados X, Y. Teniendo en cuenta los vectores unitarios ix, jy y
conociendo que que U y V son ortogonales.

U = ( U cos(θ )) i x + ( U sen(θ )) jy
U = ( U • i x ) i x + ( U • jy ) jy
V = ( V cos(90 + θ )) i x + ( V sen(90 + θ )) jy 6
V = ( V • i x ) i x + ( V • jy ) jy
V = (− V sen(θ )) i x + ( V cos(θ )) jy
Representación de la Orientación Mediante
Matrices de Rotación. Caso 2D

7
Representación de la Orientación Mediante
Matrices de Rotación. Caso 2D

P = pui u + pv jv P = px i x + py jy

pu = P ⋅ i u px = P ⋅ i x 8
pv = P ⋅ jv py = P ⋅ jy
€ €
Representación de la Orientación Mediante
Matrices de Rotación. Caso 2D

9
Representación de la orientación mediante
Matrices de Rotación. Caso 2D

P = px i x + py jy P = pui u + pv jv

px = P ⋅ i x = ( pui u + pv jv ) ⋅ i x
py = P ⋅€jy = ( pui u + pv jv ) ⋅ jy

px = pui u ⋅ i x + pv jv ⋅ i x " px % " pu %

$ p ' = R$ p '
€ € py = pui u ⋅ jy + pv jv ⋅ jy # y& # v&

" px % "i x ⋅ i u i x ⋅ jv % " pu % $cosθ −senθ '

$ p ' = $j ⋅ i R= &
€ # y& # y u jy ⋅ jv '& $# pv '& €
% senθ cos θ )
(
10

€
Representación de la Orientación Mediante
Matrices de Rotación. Caso 3D

11
 Matrices de Rotación. Composición
de Rotaciones
Concatenación de rotaciones Multiplicación de matrices

Orden de la composición: Las rotaciones que se especifican con

Rotación sobre OX respecto a los EJES FIJOS se
Rotación sobre OY PREMULTIPLICAN
Rotación sobre OZ

12
Matrices de Rotación. Interpretación
Geométrica
Si {xyz}R{uvw} representa la matriz de rotación que relaciona el sistema {UVW}
con el sistema {XYZ}

(i.e pxyz={xyz}R{uvw} puvw)

Las columnas de la Matriz {xyz}R{uvw} corresponden con las coordenadas de

los vectores u,v,w en la base {XYZ}

ux vx wx
{xyz}R
{uvw} = uy vy wy
uz vz wz
13
Matrices de Rotación.
Interpretación Geométrica
ux vx wx 0 0 1
{xyz}R
{uvw} = uy vy wy = 0 -1 0
uz vz wz 1 0 0

14
Representación conjunta
mediante matrices

15
Matrices de transformación homogénea
•  Matriz 4x4 que representa la transformación de un vector en coordenadas
homogéneas de un sistema de coordenadas a otro

⎡ R 3x3 p3x1⎤ ⎡ Rotacion Traslacion⎤

T=⎢ ⎥=⎢ ⎥
⎢⎣ f 1x3 w1x1⎥⎦ ⎣ Perspectiva Escalado ⎦

•  R3x3: matriz de rotación
•  p3x1: vector de traslación
•  f1x3: transformación de perspecEva ((0,0,0) en el caso de robóEca)
•  w1x1: escalado global (1 en el caso de robóEca)

! $
# Rotación 3x3 Traslación 3x1 &
T= 16
# 0 0 0 1 &
" %
MTH Para la Nigura de abajo, encuentre las matrices de transformación de
4x4 y para i=1, 2, 3, 4, 5
i −1 0
Ai Ai
⎡ nx sx ax px ⎤
⎢n sy ay p y ⎥⎥ " %
F =⎢ y −1 0 0 0
⎢ nz sz az pz ⎥ $ '
0 0 −1 e + c
⎢ ⎥ 0
A1 = $ '
⎣0 0 0 1⎦ $ 0 −1 0 a − d '
c $ 0 0 0 1 '
# &
z3
b y3
⎡0 − 1 0 b ⎤
x3 ⎢0 0 − 1 a − d ⎥
z5 d
1
x5
A2 = ⎢ ⎥
⎢1 0 0 0 ⎥
y5 z4 ⎢ ⎥
e
⎣ 0 0 0 1 ⎦
a x4 y4 z2 ⎡0 1 0 −b ⎤
x2 ⎢− 1
z1 x1 y2 0 0 0 e + c ⎥⎥
A2 = ⎢ 17
z0
y1
⎢0 0 1 0 ⎥
⎢ ⎥
x0
y0 ⎣0 0 0 1 ⎦
Aprender Haciendo

18
Representación de un Sistema
por medio de MTHs

! 1 0 0 250 $ " 1 0 0 −250 %

# & $ '
0 1 0 100 & 0 1 0 −100 '
0
A1 = # 1
A0 = $
# 0 0 1 50 & $ 0 0 1 −50 ' 19
# 0 0 0 1 &% $ 0 0 0 1 '&
" #
Representación de un Sistema
por medio de MTHs
% Diap_Representacion_MTH.m!
!
clear, close all, clc!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [ 1, 0, 0, 250;!
0, 1, 0, 100;!
0, 0, 1, 50;!
0, 0, 0, 1];!
!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
view(40,20);, grid on;, hold on;!

20
Representación de un Sistema
por medio de MTHs

" " 0 0 250 %

−1 0 0 250 % $
−1
'
$ '
0 0 1 100 ' 0 0 −1 50 '
0
A1 = $
1
A0 = $
$ 0 −1 0 50 ' $ 0 1 0 −100 ' 21
$ $ 1 '&
# 0 0 0 1 '& # 0 0 0
Representación de un Sistema
por medio de MTHs
% Diap_Representacion_MTH2.m!
!
clear, close all, clc!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [-1, 0, 0, 250;!
0, 0, 1, 100;!
0, -1, 0, 50;!
0, 0, 0, 1];!
!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
view(40,20);, grid on;, hold on;!

22
Traslación: Pre-multiplicación

0
A2 = MTHtray(300) 0 A1

! 1 0 0 0 $! −1 0 0 250 $ ! −1 0 0 250 $
# &# & # &
0
A2 = #
0 1 0 300 &# 0 0 1 100 & # 0 0 1 400 &
= 23
# 0 0 1 0 &# 0 −1 0 50 & # 0 −1 0 50 &
# 0 0 0 1 &%#" 0 0 0 1 &% #" 0 0 0 1 &%
"
Traslación: Pre-multiplicación
% Diap_Traslacion.m!
!
clear, close all, clc!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [-1, 0, 0, 250;!
0, 0, 1, 100;!
0, -1, 0, 50;!
0, 0, 0, 1];!
A02 = MTHtrasy(300)*A01;!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
dibujar_sistema_referencia_MTH(A02, 70, 5, '2');!
view(40,20);, grid on;, hold on;!

24
Traslación: Pos-multiplicación

0
A2 = 0 A1MTHtray(300)

" −1 0 0 250 %" 1 0 0 0 % " −1 0 0 250 %

$ '$ ' $ '
0
A2 = $
0 0 1 100 '$ 0 1 0 300 ' $ 0 0 1 100 '
= 25
$ 0 −1 0 50 '$ 0 0 1 0 ' $ 0 −1 0 −250 '
$ 0 0 0 1 '&$# 0 0 0 1 '& $# 0 0 0 1 '&
#
Traslación: Pos-multiplicación
% Diap_Traslacion_Pos.m!
!
clear, close all, clc!
f1 = figure(1);, set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [-1, 0, 0, 250;!
0, 0, 1, 100;!
0, -1, 0, 50;!
0, 0, 0, 1];!
A02 = A01*MTHtrasy(300);!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
dibujar_sistema_referencia_MTH(A02, 70, 5, '2');!
view(40,20);, grid on;, hold on;!

26
 Rotación: Pre-multiplicación

0
A2 = MTHrotz(π ) 0 A1

" cos(π ) −sen(π ) %"

$
0 0
'$ −1 0 0 250 % " 1 0 0 −250 %
' $ '
0
A2 = $
sen(π ) cos(π ) 0 0 '$ 0 0 1 100 ' $ 0 0 −1 −100 '
= 27
$ 0 0 1 0 '$ 0 −1 0 50 ' $ 0 −1 0 50 '
$ '$
# 0 0 0 1 &# 0 0 0 1 '& $# 0 0 0 1 '&
Traslación: Pre-multiplicación
% Diap_Rotacion_Pre.m!
!
clear, close all, clc!
f1 = figure(1);, set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [-1, 0, 0, 250;!
0, 0, 1, 100;!
0, -1, 0, 50;!
0, 0, 0, 1];!
A02 = MTHRotz(pi)*A01;!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
dibujar_sistema_referencia_MTH(A02, 70, 5, '2');!
view(40,60);, grid on;, hold on;!
!

28
 Rotación: Pos-multiplicación

0
A2 = 0 A1MTHrotz(π )

" " % "

−1 0 0 250 %$ cos(π ) −sen(π ) 0 0
' $ 1 0 0 250 %
$ ' '
0
A2 = $
0 0 1 100 '$ sen(π ) cos(π ) 0 0 '=$ 0 0 1 100 ' 29
$ 0 −1 0 50 '$ 0 0 1 0 ' $ 0 1 0 50 '
$ $ ' $
# 0 0 0 1 '&# 0 0 0 1 & # 0 0 0 1 '&
Traslación: Pos-multiplicación
% Diap_Rotacion_Pos.m!
!
clear, close all, clc!
f1 = figure(1);, set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = [-1, 0, 0, 250;!
0, 0, 1, 100;!
0, -1, 0, 50;!
0, 0, 0, 1];!
A02 = A01*MTHRotz(pi);!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
dibujar_sistema_referencia_MTH(A02, 70, 5, '2');!
view(40,60);, grid on;, hold on;!
!

30
 Concatenación MTH
1.  Traslación a lo largo del eje x fijo 250 unidades
2.  Rotación alrededor del eje z fijo un ángulo de 180 grados
3.  Traslación a lo largo del eje y móvil 400 unidades

1
0
A1 = MTHtrasx(250)

2 1
0
A2 = MTHrotz(π )MTHtrasx(250)

2 1 3
0
A3 = ( MTHrotz(π )MTHtrasx(250)) MTHtrasy(400)
Concatenación MTH
% Diap_Concatenacion.m!
!
clear, close all, clc!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
A00 = eye(4);!
A01 = MTHtrasx(250);!
A02 = MTHRotz(pi)*MTHtrasx(250);!
A03 = MTHRotz(pi)*MTHtrasx(250)*MTHtrasy(400);!
dibujar_sistema_referencia_MTH(A00, 70, 5, '0');!
dibujar_sistema_referencia_MTH(A01, 70, 5, '1');!
dibujar_sistema_referencia_MTH(A02, 70, 5, '2');!
dibujar_sistema_referencia_MTH(A03, 70, 5, '3');!
view(40,60);, grid on;, hold on;!
!
axis([-400 400 -500 300 0 80]);!
!

32
Generar animaciones del movimiento
usando MTH

33
Traslación de un sistema coordenado
alrededor de su eje z

34
Traslación de un sistema coordenado
alrededor de su eje z

% Ejemplo_Animacion_traslacion_MTH_1.m!
%!
clear, close all, clc!
!
% Color de fondo!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
!
% traslacion eje Z!
for L=0:2:50 !
figure(1), clf;!
A00 = eye(4);!
A = MTHtrasz(L); !
dibujar_sistema_referencia_MTH(A00, 25, 5, '0');!
dibujar_sistema_referencia_MTH(A, 25, 5, '1'); !
axis([ -60 60 -60 60 -60 80]);!
grid on;!
view(40,20), camlight(40,20);, lighting phong;!
pause(0.2) !
end!
! 35
Traslación de un sistema coordenado
alrededor de su eje x móvil (conEnuación)

36
Traslación de un sistema coordenado
alrededor de su eje x móvil (conEnuación)

% Ejemplo_Animacion_traslacion_MTH_2.m!
%!
clear, close all, clc!
!
% Color de fondo!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
!
% traslacion eje x (movil)!
for L2=0:2:50!
figure(1), clf;!
A00 = eye(4);!
A = MTHtrasz(50)*MTHtrasx(L2);!
dibujar_sistema_referencia_MTH(A00, 25, 5, '0');!
dibujar_sistema_referencia_MTH(A, 25, 5, '1'); !
axis([ -60 60 -60 60 -60 80]);!
grid on;!
view(40,20), camlight(40,20);, lighting phong;!
pause(0.2) !
end!
! 37
Rotación de un sistema coordenado
alrededor de su eje x móvil (conEnuación)

38
Rotación de un sistema coordenado
alrededor de su eje x móvil (conEnuación)
% Ejemplo_Animacion_MTH.m!
%!
clear, close all, clc!
!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
!
for theta=0:pi/20:2*pi!
!
figure(1), clf;!
A00 = eye(4);!
A = MTHtrasz(50)*MTHtrasx(50)*MTHRotx(theta);!
dibujar_sistema_referencia_MTH(A00, 25, 5, '0');!
dibujar_sistema_referencia_MTH(A, 25, 5, '1');!
!
axis([ -60 60 -60 60 -60 60]);!
grid on;!
view(40,20), camlight(40,20);, lighting phong;!
!
pause(0.2) !
end!
!
39
Dibujar/Importar Geometrías 3D en
Matlab/Octave

40
Dibujar una cara de objeto
%ejemplo dibujo de un triangulo usando la funcion patch!
f3=figure(3), clf, set(f3,'Color',[1, 1, 1]); !
hold on, grid on;!
% Puntos de los vertices de la cara!
p1 = [0,0,0];!
p2 = [1,0,0];!
p3 = [0.5,1,1];!
% Dibuja la Cara!
patchi=patch([p1(1), p2(1), p3(1)], [p1(2), p2(2), p3(2)], ...!
[p1(3), p2(3), p3(3)],'g'); !
% Establece el color de los bordes!
set(patchi, 'EdgeColor', 'k' ); !
xlabel('x','FontSize',15); !
ylabel('y','FontSize',15); !
zlabel('z','FontSize',15); !
view(55,15)!

41
 Importar graficos STL a Matlab/Octave
solid ascii!
facet normal 1.000000e+000 0.000000e+000 0.000000e+000!
outer loop!
vertex 2.800000e+001 -1.100000e+001 1.000000e+001!
vertex 2.800000e+001 -1.100000e+001 0.000000e+000!
vertex 2.800000e+001 2.800000e+001 1.000000e+001!
endloop!
endfacet!
facet normal 1.000000e+000 0.000000e+000 0.000000e+000!
outer loop!
vertex 2.800000e+001 2.800000e+001 1.000000e+001!
vertex 2.800000e+001 -1.100000e+001 0.000000e+000!
vertex 2.800000e+001 2.800000e+001 0.000000e+000!
endloop!
endfacet!
...

for i = 2:7:nfil-1 % encabezado!

n_face = n_face +1;!
Cf = textscan(datos{i}, '%s %s %n %n %n');!
C1 = textscan(datos{i+2}, '%s %n %n %n');!
C2 = textscan(datos{i+3}, '%s %n %n %n');!
C3 = textscan(datos{i+4}, '%s %n %n %n');!
datos_mat.facet_normal(n_face,:) = [Cf{3}, Cf{4}, Cf{5}];!
datos_mat.vertex1(n_face,:) = [C1{2}, C1{3}, C1{4}];!
datos_mat.vertex2(n_face,:) = [C2{2}, C2{3}, C2{4}];!
datos_mat.vertex3(n_face,:) = [C3{2}, C3{3}, C3{4}];!
end!
Importar gráNicos STL a
Matlab
function objeto_matlab = fun_stl2matlab(file, color, dibujar)!
!

% fun_stl2matlab.m!
% importa datos de un fichero stl!
% Ejemplo:!
!
!
clear, close all, clc!
objeto_matlab = fun_stl2matlab('cubostl.stl', [1,0,0],1)!
!

43
Dibujar gráNicos importados
!

de STL a Matlab
function dibujar_objeto_matlab_from_stl(objeto,A)!
!

% dibujar_objeto_matlab_from_stl.m!
% Esta funcion dibuja un objeto importado de STL segun la !
% la MTH A!
% Ejemplo:!
clear, close all, clc!
objeto = fun_stl2matlab('fig3d1.stl', [1,0,0],0);!
A = [ 1, 0, 0, 30; ...!
0, cos(pi/6), -sin(pi/6), 20; ...!
0, sin(pi/6), cos(pi/6), 50; ...!
0, 0, 0, 1];!
dibujar_objeto_matlab_from_stl(objeto,A);!
view(40,20), camlight(40,20);, lighting phong; !

44
Rotación de una geometría
alrededor de su eje x (móvil)

45
Rotación de una geometría
alrededor de su eje x (móvil)
% Ejemplo_Animacion_STL.m!
%!
clear, close all, clc!
!
% Carga la geometria en Matlab!
objeto = fun_stl2matlab('fig3d1.stl', [1,0,0],0);!
% Color de fondo!
f1 = figure(1), set(f1, 'Color', [1,1,1]), clf!
!
for theta=0:pi/20:2*pi !
figure(1), clf;!
A00 = eye(4);!
A = MTHtrasz(50)*MTHtrasx(50)*MTHRotx(theta); !
dibujar_sistema_referencia_MTH(A00, 25, 5, '0');!
dibujar_sistema_referencia_MTH(A, 50, 5, '1');!
!
dibujar_objeto_matlab_from_stl(objeto,A);!
!
axis([ -60 60 -60 60 -60 60]);!
grid on;!
view(40,20), camlight(40,20);, lighting phong;!

end!
pause(0.2)! 46
!
 Evolución MTH

47

Código: Ejemplo_Animacion_traslacion_Rotacion_STL.m