Modelo cinemtico de la interface hptica

PHANToM Premium 1.5/6DOF

Adolfo Rodrguez Tsouroukdissian, Luis Basaez
Villaluenga

IOC-DT-P-2005-6
Febrer 2005

Modelo cinemtico de la interfase hptica PHANToMTM

Premium 1.5/6DOF
Adolfo Rodrguez Tsouroukdissian
Luis Basaez Villaluenga
Instituto de Organizacin y Control
Universidad Politcnica de Catalua
02 de febrero de 2005
posicionamiento del elemento terminal, y las tres
ltimas de su orientacin.

1. RESUMEN
En el presente reporte se deriva la cinemtica directa
e inversa, as como el Jacobiano de la interfase hptica
PHANToMTM Premium 1.5/6DOF.

2. CINEMTICA
La configuracin inicial del manipulador y la
nomenclatura utilizada se muestran en la figura 1.
Dado que los ejes de rotacin de las tres ltimas
articulaciones se interceptan en un mismo punto, el
manipulador es de tipo wrist-partitioned. Esto es, las
tres primeras articulaciones se encargan del

El problema cinemtico de este tipo de manipulador

se puede descomponer en dos problemas
independientes, los de obtener el mapa directo e
inverso entre:
- Los ngulos 1, 2, 3 y la posicin del elemento
terminal, medida con respecto al sistema base.
- Los ngulos 4, 5, 6 y la orientacin del
elemento terminal, medida con respecto al sistema
base.
Las transformaciones de cuerpo rgido y clculos
cinemticos siguen la notacin de [2].

l1

2
1

l2

6
5

Figura 1. Configuracin inicial del manipulador y nomenclatura utilizada

z1
y1

y3
x1

x3

z3

y2

z2

y0

x2

y4

x0

z0

sin ( 5 ) cos( 5 )
0
0
4

T
=
5
cos( 5 ) sin ( 5 )

0
0

0 0
1 0
0 0

0 1
0 0
1 0
0 0

0 1

z5
x4

z4

y6

y5
x5

x6

z6

Figura 2. Sistemas de coordenadas en la configuracin inicial

2.1.

Cinemtica Directa

Los sistemas de coordenadas asociados a cada

articulacin y el sistema base se muestran en la
figura 2 .
Las transformaciones homogneas entre el sistema
{i} y el sistema {i-1}, para i {1,..,6} son
sin (1 ) cos(1 ) 0 0
0
0
1 l 2
0

T
=
1
cos(1 ) sin (1 ) 0 l1

0
0 1
0
cos( 2 ) sin ( 2 ) 0 0
0
1 0
0
1

T
=
2
sin ( 2 ) cos( 2 ) 0 0

0
0 1
0
cos( 3 2 ) sin ( 3 2 ) 0 l1
sin ( ) cos( ) 0 0
3
2
3
2
2

3T =

0
0
1 0

0
0
0 1

0
cos( 4 ) sin ( 4 ) 0
0
0
1 l 2
3

T
=
4
sin ( 4 ) cos( 4 ) 0
0

0
0
1
0

(1)

sin ( 6 ) cos( 6 )

0
0
5

=
T
6
cos( 6 ) sin ( 6 )

0
0

(3)

(4)

(6)

La transformacin homognea entre el sistema {6}

y el sistema {0} se obtiene a partir de la composicin
0
0
1 2
3
4
5
6T = 1T 2T 3T 4T 5T 6T

(2)

(5)

r11
r
0
21
=
T
6
r31

r12
r22
r32
0

r13
r23
r33
0

px
p y
=
pz

1 0

(7)

0
6 d

0
6R

(8)

Por razones de simplicidad y brevedad, se adoptar

en lo que resta de documento la notacin sin ( i ) = si
y cos( i ) = ci ,
para las posiciones de las
articulaciones y sin ( ) = s y cos( ) = c para los
dems ngulos.

r11 = s1c3 c4 s5 s6 s1 s3 c5 s6 + s1c3 s4 c6

(9)

+ c1 s4 s5 s6 + c1c4 c6

r12 = s1c3 c4 s5 c6 s1 s3 c5 c6 s1c3 s4 s6

En el intervalo ( 2 , 2 ) la solucin de (23) tiene la

forma
= atan2 r23 , 1 r23 2 .

(10)

+ c1 s4 s5 c6 c1c4 s6
r13 = s1c 3 c 4 c 5 s1 s 3 s 5 c1 s 4 c 5
r21 = s3c4 s5 s6 + s3s4c6 + c3c5 s6
r22 = s3c4 s5c6 s3s4 s6 + c3c5c6
r23 = s 3 c 4 c 5 + c 3 s 5

(11)
(12)
(13)
(14)

r31 = c1c3 c4 s5 s6 c1 s3 c5 s6 + c1c3 s4 c6

Si { 2 , 2}
r r
= atan2 13 , 33
c c

r21 r22
.
= atan2
,
c c

(15)

s1 s4 s5 s6 s1c4 c6
r32 = c1c3 c4 s5 c6 c1 s3 c5 c6 c1c3 s4 s6

(16)

s1 s4 s5 c6 + s1c4 s6
r33 = c1c 3 c 4 c 5 c1 s 3 s 5 + s1 s 4 c 5
p x = s1 (l1c 2 + l 2 s 3 )
p y = l1 s 2 + l 2 (1 c 3 )
p z = l1 (c1 c 2 1) + l 2 (c1 s 3 )

(20)

c + s + 0

RE = 0
0
1
s + c + 0

+ = atan2( r12 ,r32 ) .

0
6d

directamente por los parmetros del vector

(ecuaciones (18), (19) y (20) respectivamente).

= atan2( r12 ,r32 ) .

La orientacin del elemento terminal est expresada

en trminos de los ngulos de Euler YXZ. Estos
corresponden a las siguientes rotaciones sucesivas:
- Rotacin alrededor del eje z
- Rotacin alrededor del eje x
- Rotacin alrededor del eje y
La matriz de rotacin asociada a estos ngulos es
s c

s (21)
cc

Los ngulos , , se obtienen igualando (21) y la

matriz de rotacin contenida en (8)
(22)

se calcula a partir de
= asin (r23 ) .

(23)

(27)
(28)

Dado que (28) tiene infinitas soluciones para y ,

se impone la restriccin adicional = 0 y se obtiene

Orientacin

R E = 60 R .

(26)

En = { 2 , 2} , (25) y (26) no estn definidas.

Evaluando (21) en = 2

La posicin del elemento terminal en coordenadas

del sistema base p x0 , p y0 , p z0 viene dada

s sc c s
cc
c sc + s s

(25)

(17)
(18)
(19)

Posicin

s s s + c c

RE =
c s
c s s s c

(24)

(29)

Anlogamente, evaluando (21) en = 2

c
s 0

RE = 0
0
1
s c 0

= atan2(r12 , r32 )

(30)
(31)

Imponiendo la restriccin adicional = 0

= atan2(r12 , r32 )

2.2.

(32)

Cinemtica Inversa

Posicin
La metodologa descrita en este apartado sigue el
esquema propuesto por [1].
A partir de la vista superior del manipulador
mostrada en la figura 3 se obtiene 1

1 = atan2 p x0 , p z0 + l1 .

(33)

py0
2

l2

pz0

l1

px0
1

Figura 3: Vistas lateral y superior del manipulador en una posicin genrica

Para calcular 2 y 3
figuras 3 y 4

se tiene, segn las

(p z + l1 ) + p x
r = ( p y l 2 )2 + R 2
= atan2( p y l 2 , R ) .

R=

(34)

Orientacin
Para que el elemento terminal est orientado segn
los ngulos de Euler YXZ se debe satisfacer (22)
0
6 R = RE
0
4
3 R 6 R = RE
4
0 1
RE
6 R= 3 R
4
0 T
6 R= 3 R R E

(35)
(36)

A partir del teorema del coseno del tringulo

superior de la figura 4 se obtienen los ngulos y
l 2 + l2 2 r 2
= acos 1

2l1l 2

(37)

l 2 l2 2 + r 2
.
= acos 1

2l1r

(38)

(42)
(43)
(44)

l2

l1

Dado que los ngulos y son siempre positivos

en el espacio de trabajo del manipulador, 2 y 3 se
pueden expresar como
2 = +
3 = 2 + 2 .

(41)

(39)
(40)

py0 - l2

Figura 4: Diagrama utilizado en el clculo de la cinemtica

inversa

donde
se obtiene
4
6R

c 4 s 5 s 6 + s 4 c 6
c5 s 6
=
s 4 s 5 s 6 c 4 c 6

c 4 s5 c6 s 4 s6
c5 c6
s 4 s5 c6 + c4 s6

a11

0 T
3 R R E = a 21
a31

a12
a 22
a32

c 4 c5
s 5 (45)
s 4 c 5

a13
a 23
a33

a11 = c1c3 ( c s s s c )

(47)

+ s1c3 ( s s s + c c ) + s3 c s
a12 = c1c3 ( c s c + s s )

(48)

+ s1c3 ( s s c c s ) + s3 c c

a13 = c1 c 3 c c + s1c 3 s c s 3 s

(49)

a21 = c1 s3 ( c s s s c )

(50)

s1 s3 ( s s s + c c ) + c3 c s
a22 = c1 s3 ( c s c + s s

(46)

s1 s3 ( s s c c s ) + c3 c c

(51)

a 23 = c1 s 3 c c s1 s 3 s c c 3 s

(52)

a 31 = s1 c s s s c c1 s s s + c c

(53)

(
) (
)
a 32 = s1 (c s c + s s ) c1 (s s c c s )
a 33 = s1c c c1 s c

(54)
(55)

4 + 6 = atan2(a 32 ,a 31 )

(61)

Para que existan 4 y 6 nicos para un 5 dado, se

combina (61) con unas restricciones adicionales que
dan preferencia al movimiento de la articulacin 6
con respecto al de la articulacin 4:
Si 6 min 6 max
4 = 0
6 =

(62)
(63)

Si < 6 min
4 = 6 min
6 = 6 min

(64)
(65)

Si > 6 max
4 = 6 max
6 = 6 max

(66)
(67)

donde = atan2(a 32 ,a 31 ) .
El caso singular 5 = 2 no ser analizado ya
que representa una configuracin imposible del
manipulador.
Dado que (56) tiene dos soluciones en el intervalo

[ , ] , se pueden calcular dos conjuntos de ngulos

5 se calcula como
5 = asin (a 23 ) .

(56)

En el intervalo ( 2 , 2 ) la solucin de (56)

tiene la forma
5 = atan2 a 23 , 1 a 23 2

(57)

Si 5 2 , 4 y 6 quedan como

4, 5, 6
que satisfagan la orientacin definida por los ngulos
de
Euler YXZ.
El primer conjunto ha sido calculado para
5 [0, 2) , y el segundo, llamado 4, 5, 6 se

calcula para 5 ( 2 , ] segn

5 * = atan2 a 23 , 1 a 23 2

(68)

a
a
4 = atan2 33 , 13
c5 c5

(58)

4 * = a tan 2( s 4 ,c 4 )

(69)

a
a
6 = atan2 21 , 22
c
5 c5

(59)

6 * = a tan 2( s 6 ,c6 )

(70)

En 5 = 2 (58) y (59) no estn definidas ya

que existe una singularidad de mueca. Evaluando
(45) en 5 = 2

4
6R

s 4+6
= 0
c 4+ 6

c 4+6
0
s 4+6

0
1
0

(60)

3. JACOBIANO

J 6

El Jacobiano J de un manipulador de n
articulaciones es una matriz que relaciona las
velocidades
de
las
articulaciones
T
& = [& 1 & 2 ... & n ]
con la velocidad del
elemento terminal. El Jacobiano est compuesto por
dos submatrices:
- J que relaciona las velocidades angulares de las
articulaciones con la velocidad angular del
elemento terminal.
0
&
(71)
n = J
- Jv que relaciona las velocidades angulares de las
articulaciones con la velocidad lineal del
elemento terminal.
0
&
(72)
n v = Jv
El Jacobiano tiene la forma
J
J = v
J

(73)

El manipulador en consideracin
articulaciones, por lo tanto n=6.

tiene

J
El Jacobiano de velocidades angulares J est
compuesto por vectores columna Ji, cada uno
correspondiente al vector ki = [0 0 1]T asociado
al sistema {i}, expresado en coordenadas del sistema
{0}
J = [J 1

J i = 0i R ki
J 2

J 3

J 4

(74)

J 5

J 6 ] (75)

donde

J 2

J 5

0
J 1 = 1
0
c1
= J 3 = 0
s1

s1 s 3
J 4 = c 3
c1 s 3
s1 c 3 s 4 c1c 4

=
s3 s 4

c1 c 3 s 4 + s1c 4

(76)

(77)

(78)

(79)

s1c 3 c 4 c 5 c1 s 4 c 5 s1 s 3 s 5

s3 c 4 c5 + c3 s5
=

c1c 3 c 4 c 5 + s1 s 4 c 5 c1 s 3 s 5

(80)

Jv
El Jacobiano de velocidades lineales Jv est
compuesto por vectores columna Jvi, cada uno
correspondiente al producto vectorial
J v = [J v1

J vi = J i
J v2

J v3

( d d)
0
6

0
i

J v4

J v5

J v6 ]

(81)
(82)

J i es el vector unitario orientado segn el eje de

giro del sistema {i}, y 60 d 0i d el vector entre los

orgenes de los sistemas {i} y {n}. Ambos vectores
estn expresados en coordenadas del sistema {0}.
Los distintos vectores 0i d tienen la forma
0
= l 2
l1
l1 s1 c 2
l s +l
0
d
=
3
1 2 2
l1 (c1 c 2 1)
s1 (l1 c 2 + l 2 s 3 )

0
0
0
4 d = 5 d = 6 d = l1 s 2 + l 2 (1 c 3 )
l1 (c1 c 2 1) + l 2 c1 s 3
0
0
1d=2d

(83)

(84)

(85)

y finalmente
c1 (l1c 2 + l 2 s 3 )

J v1 =
0

s1 (l1c 2 + l 2 s 3 )
s1 ( l1 s 2 + l 2 c 3 )
J v 2 = l1 c 2 + l 2 s 3
c1 ( l1 s 2 + l 2 c 3 )
J v3

l 2 s1 c 3
= l 2 s 3
l 2 c1 c 3

J v 4 = J v5 = J v6

0
= 0
0

(86)

(87)

(88)

(89)

REFERENCIAS
4. ESPACIO DE TRABAJO
Los datos utilizados en los clculos cinemticos as
como las soluciones obtenidas sern vlidos siempre
y cuando pertenezcan al espacio de trabajo del
manipulador. Este espacio de trabajo viene definido
por las longitudes l1 y l2, y el rango angular de cada
una de las articulaciones i , con i {1,..,6} .

[1] avuolu, M. C. & Feygin, D. (2001).

Kinematics and dynamics of PHANToM model
1.5 haptic interface (Tech. Rep.). University of
California at Berkley, Electronics Research
Laboratory Memo M01/15
[2] Craig, J. J. (1989). Introduction to robotics
(Second ed.) Adison-Wesley. Reading. MA.