Simulador 3D de Robots

Ejemplo RAPLIM

Ph.D. César Augusto Peña Cortés

Raplim

Robot asistente para personas con

problemas de movilidad

Tesis del programa de Ing. Mecatrónica

Autores: Ing. Alfredo Márquez, Ing. Javier Hernández
Director: PhD. César Peña
Raplim – Arquitectura del Sistema
Raplim – Video demostrativo
Objetivo del taller:
Crear un simulador del sistema RAPLIM
Raplim – Características
Paso 1: Dibujar/Importar Geometrías 3D en Matlab
Dibujar una cara de objeto en Matlab

%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)
 Importar graficos STL a Matlab

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áficos STL a Matlab

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

function objeto_matlab = fun_stl2matlab(file, color, dibujar)

datos = importdata(file,'\t');
[nfil,nada]=size(datos);
n_face = 0;
datos_mat=[];
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áficos STL a Matlab

objeto_matlab.x = [datos_mat.vertex1(:,1)'; datos_mat.vertex2(:,1)'; datos_mat.vertex3(:,1)'];

objeto_matlab.y = [datos_mat.vertex1(:,2)'; datos_mat.vertex2(:,2)'; datos_mat.vertex3(:,2)'];
objeto_matlab.z = [datos_mat.vertex1(:,3)'; datos_mat.vertex2(:,3)'; datos_mat.vertex3(:,3)'];
objeto_matlab.tcolor(1,1:n_face,1) = color(1);
objeto_matlab.tcolor(1,1:n_face,2) = color(2);
objeto_matlab.tcolor(1,1:n_face,3) = color(3);
objeto_matlab.n_faces = n_face;
if dibujar
f1=figure;, set(f1,'Color',[1, 1, 1]);
p=patch(objeto_matlab.x,objeto_matlab.y,objeto_matlab.z,objeto_matlab.tcolor);
set(p, 'EdgeColor', 'none' );
view(40,20), camlight(40,20);, lighting phong;
xlabel('x');, ylabel('y');, zlabel('z');, grid on
end
Dibujar gráficos importados de STL a Matlab

% 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;

function dibujar_objeto_matlab_from_stl(objeto,A)

for i=1:objeto.n_faces
for j=1:3
p = [ objeto.x(j,i); objeto.y(j,i); objeto.z(j,i); 1];
pa = A*p;
objeto_A.x(j,i) = pa(1);
objeto_A.y(j,i) = pa(2);
objeto_A.z(j,i) = pa(3);
end
end
p=patch(objeto_A.x,objeto_A.y,objeto_A.z,objeto.tcolor);
set(p, 'EdgeColor', 'none' );
Paso 2: Generar animaciones del movimiento de las
geometrías usando MTH
Rotación de un sistema coordenado
alrededor de su eje x
Rotación de un sistema coordenado
alrededor de su eje x

% 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, 50, 5, '1');

axis([ -60 60 -60 60 -60 60]);

grid on;
view(40,20), camlight(40,20);, lighting phong;

pause(0.2)
end
Rotación de una geometría alrededor de su eje x
Rotación de un sistema coordenado
alrededor de su eje x
% 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([ -100 100 -100 100 -100 60]);

grid on;
view(40,20), camlight(40,20);, lighting phong;

pause(0.2)

end
Paso 3: Dibujar el robot en alambres
Dibujar los sistemas coordenados de los eslabones del robot
Dibujar los sistemas coordenados de los eslabones del robot ( Método no
sistemático
% Diap_raplim_dibujar_robot_alambres_metodo_no_sistematico_part_1.m
clear, close all, clc

% Coordenadas articulares del robot

q = [0,0,0,0];

% almacena las dimensiones en la estructura del robot

raplim.dimensiones.L1a = 85;
raplim.dimensiones.L1b = 50.5;
raplim.dimensiones.L1 = raplim.dimensiones.L1a+raplim.dimensiones.L1b;
raplim.dimensiones.L2 = 120.23;
raplim.dimensiones.L3 = 94.6;
raplim.dimensiones.L4 = 65;

% Matrices de los sistemas coordenados (Metodo no sistematico - MTH)

Am0 = eye(4); % Matriz que define la localizacion del robot
A01 = MTHRotz(q(1)+pi/2)*MTHtrasz(raplim.dimensiones.L1)*MTHRotx(pi/2);

% Transforma las matrices dado la localizacion del robot con Am0

Am1 = Am0*A01;

% dibuja los sistemas coordenados

f1=figure(1);, set(f1, 'Color', 'w');
dibujar_sistema_referencia_MTH(Am0, 50, 5, '0');
dibujar_sistema_referencia_MTH(Am1, 50, 5, '1');

grid on;, view(130,25)

Continuar dibujando los otros sistemas coordenados

Dibujar líneas

% ejemplo_dibujar_lineas.m
%
% Este programa dibuja 3 lineas

clear; % Borra las variables del workspace

close all; % cierra las ventanas abiertas
clc; % borra la pantalla
figure(1); % Crea una ventana para hacer nueva figura
p1 = [1 , 1 , 1]; % Punto 1
p2 = [3 , 4 , 5]; % Punto 2
p3 = [-3, -2, 5]; % Punto 3
% Dibuja las lineas:
line( [p1(1) , p2(1)], [p1(2) , p2(2)], [p1(3) , p2(3)], ...
'LineWidth',1, 'Color', 'r' , 'LineStyle', '--' );
line( [p1(1) , p3(1)], [p1(2) , p3(2)], [p1(3) , p3(3)], ...
'LineWidth',5, 'Color', 'g' , 'LineStyle', '-' );
line( [p2(1) , p3(1)], [p2(2) , p3(2)], [p2(3) , p3(3)], ...
'LineWidth',3, 'Color', 'b' , 'LineStyle', ':' );

title('Mi primera linea en Matlab'); % Crea el titulo de la figura

xlabel('X'); % Etiqueta el eje x
ylabel('Y'); % Etiqueta el eje y
zlabel('Z'); % Etiqueta el eje y
grid on % Activa la grilla
view(40,20) % Especifica un vista con azimut 40 y elevacion 20
Generar una función que cree una ínea entre dos puntos

% dibujar_linea.m
%
% Esta funcion se encarga de crear una línea entre dos puntos
%
% Ejemplo:
%
% p1 = [-3, -2, 5]; % Punto 1
% p2 = [10, 2, 7]; % Punto 2
% grosor = 3;
% f1=figure(1);, set(f1, 'Color', 'w');
% dibujar_linea(p1,p2,grosor);
% view(50,40);, grid on;

function dibujar_linea(p1,p2,grosor)

line( [p1(1) , p2(1)], [p1(2) , p2(2)], [p1(3) , p2(3)], ...

'LineWidth',grosor, 'Color', 'k' , 'LineStyle', '-' );
Dibujar los eslabones por medio de líneas

% dibuja las lineas correspondientes a los eslabones

grosor_eslabones = 7;
dibujar_linea(Am0(1:3,4),Am1(1:3,4),grosor_eslabones);

Continuar dibujando las otras líneas

Convertir el programa en una función

Ejercicio: convertir el programa en una función

% raplim_dibujar_robot_alambres_MNS.m
%
% Esta funcion dibuja el robot en alambres, empleando el metodo no
% sistematico para la localizacion de los sistemas coordenados de los
% eslabones
%
% Ejemplo:
%
% clear, close all, clc
% q = [pi/4,pi/6,pi/7,pi/8]; % Coordenadas articulares del robot
% raplim_dibujar_robot_alambres_MNS(q)

function raplim_dibujar_robot_alambres_MNS(q)

CONTINUAR ESCRIBIENDO LA FUNCION...

Paso 4: Importar las geometrias
 Cargar la base del robot

% raplim_cargar_stl.m (Construyendo)
clear, clc, close all

% se definen los colores de las piezas

color_raplim = [0.8,0.75,0.55];

% Carga STLs de los eslabones del robot

raplim.stl.eslabon0 = fun_stl2matlab('stl_eslabon0.stl', color_raplim,1);
dibujar_sistema_referencia_MTH(eye(4), 125, 5, '0');
title('Localizacion original eslabon 0 (Base)')

% Graba en un archivo la estructura del robot raplim

save ('raplim_estructura.mat','raplim');
Ejercicio: Cargar todas las geometrías del robot
 Función que dibuja el robot incluyendo las
geometrías y los sistemas coordenados
Programa que carga las geometrías raplim_estructura.mat
previamente y las acomoda de


acuerdo a los sistemas coordenados raplim_dibujar_robot_3d
del robot:
básicamente es la misma función
raplim_cargar_stl.m raplim_dibujar_robot_alambres_MNS.m,
pero ahora se incluye el dibujo de las geometrias en
3D


Ejercicio: Crear función raplim_dibujar_robot_3D_MNS a partir de la

función raplim_dibujar_robot_alambres_MNS

% raplim_dibujar_robot_3D_MNS.m
%
% Esta funcion dibuja el robot en 3D, empleando el metodo no
% sistematico para la localizacion de los sistemas coordenados de los
% eslabones
%
% Ejemplo:
%
% clear, close all, clc
% q = [pi/4,pi/6,pi/7,pi/8]; % Coordenadas articulares del robot
% raplim_dibujar_robot_3D_MNS(q)
% view(130,25), camlight(40,20);, lighting phong;

function raplim_dibujar_3D_alambres_MNS(q)

CONTINUAR ESCRIBIENDO LA FUNCION...

% Carga y dibuja geometrias

load('raplim_estructura.mat')
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon0 , Am0);
Aprecie que la base no esta dibujada en la localización correcta

Y debería ser así

Si se dibujan todas las geometría se obtiene

Y debería ser así

Paso 5: Dibujar las geometrías de acuerdo a los
sistemas coordenados del robot
Transformar la localización inicial de una geometría

% transforma_objeto_matlab_from_stl.m
% Esta funcion cambia la localizacion de un objeto importado de STL
% segun la MTH A
% Ejemplo:
% Geometría transformada
% clear, close all, clc
% figure(1);, set(gcf,'Color',[1, 1, 1]);
% objeto = fun_stl2matlab('stl_eslabon2.STL', [1,0,0],0);
% A = MTHtrasz(150)*MTHRotx(pi/2);
% dibujar_objeto_matlab_from_stl(objeto,eye(4));
% objeto = transforma_objeto_matlab_from_stl(objeto,A);
% dibujar_objeto_matlab_from_stl(objeto,eye(4));
% dibujar_sistema_referencia_MTH(eye(4), 225, 5, 'o');
% view(40,20), camlight(40,20);, lighting phong;
Geometría original
function objeto_A = transforma_objeto_matlab_from_stl(objeto,A)

objeto_A = objeto; % se igualan para copiar propiedades de tcolor y n_faces

for i=1:objeto.n_faces
for j=1:3
p = [ objeto.x(j,i); objeto.y(j,i); objeto.z(j,i); 1];
pa = A*p; % aplica la transformacion
objeto_A.x(j,i) = pa(1);
objeto_A.y(j,i) = pa(2);
objeto_A.z(j,i) = pa(3);
end
end


Ejercicio: Transformar la localización inicial de la base del robot para

que coincida con sistema coordenado cero
Transformar la localización inicial de la base del robot
% raplim_cargar_stl.m (Construyendo)
clear, clc, close all

% se definen los colores de las piezas

color_raplim = [0.8,0.75,0.55];
color_cuchara = [1,0,0];

% almacena las dimensiones en la estructura del robot

raplim.dimensiones.L1a = 85;
raplim.dimensiones.L1b = 50.5;
raplim.dimensiones.L1 = raplim.dimensiones.L1a+raplim.dimensiones.L1b;

% Carga STLs de los eslabones del robot

raplim.stl.eslabon0 = fun_stl2matlab('stl_eslabon0.stl', color_raplim,1);
dibujar_sistema_referencia_MTH(eye(4), 125, 5, '0');
title('Localizacion original eslabon 0 (Base)')

% transforma los objetos para que queden de acuerdo a los sistemas

% coordenados
A0CAD0 = MTHtrasz(raplim.dimensiones.L1a);
raplim.stl.eslabon0 = transforma_objeto_matlab_from_stl(raplim.stl.eslabon0,A0CAD0);

% Graba en un archivo la estructura del robot raplim

save ('raplim_estructura.mat','raplim');

% Dibuja eslabon 0 transformado

f10 = figure(10);, set(f10, 'Color', [1,1,1]);, grid on;
dibujar_objeto_matlab_from_stl(raplim.stl.eslabon0,eye(4));
view(40,20), camlight(40,20);, lighting phong;
dibujar_sistema_referencia_MTH(eye(4), 125, 5, '0');
title('Localizacion eslabon 0 (Base) - transformado');
Transformar la localización inicial de la base del robot

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS

Transformar la localización inicial del Eslabón 1

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS

Transformar la localización inicial del Eslabón 2

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS

Transformar la localización inicial del Eslabón 3

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS

Transformar la localización inicial del Eslabón 4

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS

Transformar la localización inicial del efector final
(opcional)

Al visualizar la geometría en la función:

raplim_dibujar_3D_alambres_MNS



Ejercicio: Completar el programa raplim_cargar_stl.m para que carge

todas las geometrías y las grabe en el archivo raplim_estructura.mat
Paso 6: Dibujar el robot en 3D
Dibujar el robot

Se incluye la visualización de todos los eslabones del robot en la función: raplim_dibujar_3D_alambres_MNS

% Carga y dibuja geometrias

load('raplim_estructura.mat')
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon0 , Am0);
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon1 , Am1);
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon2 , Am2);
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon3 , Am3);
dibujar_objeto_matlab_from_stl (raplim.stl.eslabon4 , Am4);
dibujar_objeto_matlab_from_stl (raplim.stl.cuchara , Am4);
Dibujar el robot incluir el efector final
Dibujar el robot (solo sistema del efector final)
Ejemplo de animación del robot moviendose

% Ejemplo_Animacion_raplim.m
%
clear;, close all;, clc;
q_ini = [-pi/2;0.7;-0;-0.7]; % Especifica posicion inicial del robot
q_fin = [0; 0; 0; 0]; % Especifica posicion final del robot
n = 20; % Especifica numero de pasos de la animacion

% Calcula puntos intermendios para cada una de las articulaciones

vec_q1 = linspace(q_ini(1),q_fin(1),n);
vec_q2 = linspace(q_ini(2),q_fin(2),n);
vec_q3 = linspace(q_ini(3),q_fin(3),n);
vec_q4 = linspace(q_ini(4),q_fin(4),n);

for i=1:n
f1 = figure(1);, clf;, set(f1, 'Color', [1,1,1]);, grid on;
% Coordenadas articulares del robot
q = [vec_q1(i);vec_q2(i);vec_q3(i);vec_q4(i)];
% Dibuja el robot
raplim_dibujar_robot_3D_MNS(q)
view(130,25), camlight(40,20);, lighting phong;
axis([ -100 400 -100 500 -50 300]);
pause(0.2)
end