Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Gustavo Armando Guancha

cdigo: 285691
Herney Felipe Cristancho
cdigo: 285828
Ingeniera Mecatrnica - Robtica
Cinemtica Directa Laboratorio 1
9 de Septiembre, 2014

1.

Ejercicio en Laboratorio

La figura 1 muestra la posicin HOME escogida para el laboratorio en la cual se

especifica la longitud de los eslabones y angulos presentes,

Figura 1: Diagrama de alambre Robot Pincher en posicin HOME elegida

Moviendo el Robot Picher manualmente se toma nota de los valores en la tabla 1
donde se consigna informacin sobre cada una de las articulaciones en la posicin HOME
antes mostrada en cuanto a su ngulo en su valor terico y el valor digital de los motores
proporcionado por RoboPlus.

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Figura 2: Fotografa Robot Pincher en posicin HOME

Cuadro 1: ngulos Posicin HOME

Articulacin
q1
q2
q3
q4
Gripper

ngulo Terico Valor de Motores

90
512
90
523
90
510
90
510
90
511

La tabla 2 muestra los valores mximos y mnimos de los ngulos por limitaciones
mecnicos pero que ha sido definido por el grupo de trabajo y llevado de forma manual.

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Cuadro 2: Tabla Limites Mecnicos

Articulacin
q1
q2
q3
q4
Gripper

Lim Sup ngulos

180
180
180
180

Lim Inf ngulos

0
0
0
0

Lim Inf Valor Motores

835
821
828
830

Lim Inf Valor Motores

218
211
212
198

Se procede a hacer el anlisis de el Robot Pincher estableciendo los ejes Z y x, con lo

cual se elabora la tabla de parmetros de Denavit-Hartenberg. ver tabla3
Cuadro 3: Tabla de Parmetros DH Modificada
No
1
2
3
4
5

a (i-1)
0
0
10,5
10,5
9,5

alpha (i-1)
0
pi /2
0
0
0

d (i)
L1+L2
0
0
0
0

theta (i)
q1
q2
q3
q4
0

Para comprobar que el anlisis de cinemtica directa se hace uso del tool box de Peter
Corke y se crea un robot con las especificaciones dadas.ver 3 y 4

Figura 3: Tabla de Parmetros DH arrojada por Matlab al crear el robot

Utilizando Matlab se calcula la matriz de transformacin homognea que relaciona la
trama 1 con la trama 5 refirindose a base-herramienta.En ella se corrobora que la ltima
fila de la matriz correspondiente a la traslacin corresponde a la suma de las longitudes
de los links en la direccin de z positiva.
Se ha implementado en Matlab tambin un script que lleva de una posicion a otra al
robot haciendo un plot cada determinado tiempo, en la figura 6 se puede observar una

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Figura 4: Plot del robot creado en Matlab con Peter Corke en posicin Home

Figura 5: Matriz de Transformacin homognea Calculada en Matlab

Figura 6: Trayectoria hasta la posicin home

secuencia de imgenes tomadas de la simulacin con el toolbox de Peter Corke que leva
de una posicin cualquiera a home.
4

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Para esta seccin se tiene en cuenta la tabla de control de los motores AX-12a que
lleva cada una de las articulaciones del robot Phantom Pincher dado que se debe conocer
el registro para dar instrucciones con el toolbox Dynamixel para Mattlab.

Figura 7: Tabla de control del motor AX-12a

a
Cdigo Matlab 1.
1
2
3
4
5
6
7
8
9
10

clear all
clc
PUERTO =16;
ID1
ID2
ID3
ID4
ID5

=
=
=
=
=

1;
2;
3;
4;
5;

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Figura 8: Fotografa Robot Pincher en posicin requerida 1

11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

q1
q2
q3
q4
q5

=
=
=
=
=

0 ;
90 ;
0 ;
0 ;
0 ;

Present_Pos = 36;
Goal_Pos = 30
MOVING_SPEED = 32;
BAUDNUM = 1;
Q1
Q2
Q3
Q4
Q5

=
=
=
=
=

angle2Dyn (
angle2Dyn (
angle2Dyn (
angle2Dyn (
angle2Dyn (

q1
q2
q3
q4
q5

)
)
)
)
)

;
;
;
;
;

loadlibrary ( dynamixel , dynamixel . h ) ;

libfunctions ( dynamixel ) ;

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Figura 9: Fotografa Robot Pincher en posicin requerida 2

Figura 10: Fotografa Robot Pincher en posicin requerida 3

31

Prueba_Coneccion = calllib ( dynamixel , dxl_initialize , PUERTO , BAUDNUM ) ;

32
33 Pos1 =
));
34 Pos2 =
));
35 Pos3 =
));
36 Pos4 =

int32 ( calllib ( dynamixel , dxl_read_word ,ID1 , Present_Posint32 ( calllib ( dynamixel , dxl_read_word ,ID2 , Present_Posint32 ( calllib ( dynamixel , dxl_read_word ,ID3 , Present_Posint32 ( calllib ( dynamixel , dxl_read_word ,ID4 , Present_Pos-

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Figura 11: Fotografa Robot Pincher en posicin requerida 4

));
37 Pos5 = int32 ( calllib ( dynamixel , dxl_read_word ,ID5 , Present_Pos));
38
39 calllib ( dynamixel , dxl_write_word ,ID1 ,32 ,56) ;
40 calllib ( dynamixel , dxl_write_word ,ID2 ,32 ,41) ;
41 calllib ( dynamixel , dxl_write_word ,ID3 ,32 ,55) ;
42 calllib ( dynamixel , dxl_write_word ,ID4 ,32 ,69) ;
43 calllib ( dynamixel , dxl_write_word ,ID5 ,32 ,106) ;
44
45 calllib ( dynamixel , dxl_write_word ,ID1 ,34 ,800) ;
46 calllib ( dynamixel , dxl_write_word ,ID2 ,34 ,800) ;
47 calllib ( dynamixel , dxl_write_word ,ID3 ,34 ,800) ;
48 calllib ( dynamixel , dxl_write_word ,ID4 ,34 ,800) ;
49 calllib ( dynamixel , dxl_write_word ,ID5 ,34 ,800) ;
50
51 calllib ( dynamixel , dxl_write_word ,ID1 ,30 , Q1 ) ;
52 calllib ( dynamixel , dxl_write_word ,ID2 ,30 , Q2 ) ;
53 calllib ( dynamixel , dxl_write_word ,ID3 ,30 , Q3 ) ;
54 calllib ( dynamixel , dxl_write_word ,ID4 ,30 , Q4 ) ;

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

55 calllib ( dynamixel , dxl_write_word ,ID5 ,30 , Q5 ) ;

56
57 [ Pos1 ; Pos2 ; Pos3 ; Pos4 ; Pos5 ]
58 calllib ( dynamixel , dxl_terminate ) ;
59 unloadlibrary ( dynamixel ) ;
60
61
pyright 2010 The MathWorks , Inc .

a
Cdigo Matlab 2
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

clear
clc

l1 = 10.5;
l2 = 10.5;
l3 = 9.5;
Q1
Q2
Q3
Q4

=
=
=
=

-20;
20;
-20;
20;

m = 30;
q1
q2
q3
q4
q5

=
=
=
=
=

( Q1 ) * pi () /180 ;
( Q2 + 90) * pi () /180 ;
( Q3 ) * pi () /180 ;
( Q4 ) * pi () /180 ;
0 ;

L1
L2
L3
L4
L5

=
=
=
=
=

Link
Link
Link
Link
Link

(
(
(
(
(

[
[
[
[
[

1
0
0
0
0

,
,
,
,
,

0
0
0
0
0

,
,
,
,
,

0 , 0 , 0 ] , modified ) ;
0 , pi () /2 , 0 ] , modified ) ;
l1 , 0 , 0 ] , modified ) ;
l2 , 0 , 0 ] , modified ) ;
l3 , 0 , 0 ] , modified ) ;

bot = SerialLink ( [ L1 , L2 , L3 , L4 , L5 ] , name , Pincher )

29 bot . fkine ( [ q1 q2 q3 q4 q5 ] ) %forward kinematics - cinematicadirecta
30 bot . plot ( [ q1 q2 q3 q4 q5 ] )
31
32 bot . jmove ()
33
34
pyright 2010 The MathWorks , Inc .
9

Informe de laboratorio de Robtica 2015-II

Laboratorio 2 1

Cdigo Matlab Clculo de matriz de trasformacin homognea y posicionamiento en

HOME
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

function [ ] = Untitled4 ()
%- - - - - - - - - - - - - - - - - - - Longitudes de Eslabones --------------------------l1 = 10.5;
l2 = 10.5;
l3 = 9.5;
h =10;
%-------- Angulos de las articulaciones dejando gripper en 0-------------Q1 = 0;
Q2 = 0;
Q3 =0;
Q4 =0;
m = 30;
q1
q2
q3
q4
q5

=
=
=
=
=

( Q1 ) * pi () /180 ;
( Q2 +90) * pi () /180 ;
( Q3 ) * pi () /180 ;
( Q4 ) * pi () /180 ;
0 ;

L1
L2
L3
L4
L5

=
=
=
=
=

Link
Link
Link
Link
Link

(
(
(
(
(

[
[
[
[
[

0
0
0
0
0

,
,
,
,
,

h
0
0
0
0

,
,
,
,
,

0 , 0 , 0 ] , modified ) ;
0 , pi () /2 , 0 ] , modified ) ;
l1 , 0 , 0 ] , modified ) ;
l2 , 0 , 0 ] , modified ) ;
l3 , 0 , 0 ] , modified ) ;

bot = SerialLink ( [ L1 , L2 , L3 , L4 , L5 ] , name , Pincher ) %

30
31 bot . fkine ( [ q1 q2 q3 q4 q5 ] ) %cinematica directa
32 bot . plot ( [ q1 q2 q3 q4 q5 ] )
33 %- - - - - - - - - - - - - - Creacin de Matrices para la Pose HOME ------------------34
35 r10 = rotz ( q1 ) ;
36 t10 = [ 0 ; 0 ; 0];
37 TR10 = rt2tr ( r10 , t10 ) ;
38
10

Informe de laboratorio de Robtica 2015-II

39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

Laboratorio 2 1

r21 = roty ( - q2 ) * rotx ( pi () /2) ;

t21 = [ 0 ; 0 ; 0];
TR21 = rt2tr ( r21 , t21 ) ;
r32 = rotz ( q3 ) ;
t32 = [ 10.5 ; 0 ; 0];
TR32 = rt2tr ( r32 , t32 ) ;
r43 = rotz ( q4 ) ;
t43 = [ 10.5 ; 0 ; 0];
TR43 = rt2tr ( r43 , t43 ) ;
r54 = rotz ( q5 ) ;
t54 = [ 9.5 ; 0 ; 0];
TR54 = rt2tr ( r54 , t54 ) ;
TR51 = TR10 * TR21 * TR32 * TR43 * TR54 %impresion de las matricescalculadas
qtg = jtraj ( [ 0 0 0 0 0 ] , [ q1 q2 q3 q4 q5 ] , m ) ;
for q = qtg
bot . plot (q ) ;
end
end

pyright 2010 The MathWorks , Inc .

Referencias
[1]

Ing. Cardenas Pedro,Universidad Nacional de Colombia,Departamento de Ingeniera

Mecanica y Mecatrnica, Gua de laboratorio No. 1- Introduccin a servomotores
Dynamixel,2014

[2]

Velez Nez,Paulina Andrea ,Universidad Carlos III de Madrid, Desarrollo de nuevos

elementos hardware para un robot modular,2011

[2]

Dynamixel Ax-12. Users Manual. Dynamixel. 2006

11