Versión “postprint” del siguiente documento publicado:
Escalera, J.A.; Abu-Dakka, F.J. Alhama, P.J.; Abderrahim, M. (2016)
Obtención del modelo dinámico simbólico de robots ramificados
utilizando grupos de Lie y grafos. En: Actas de las XXXVII Jornadas de
Automática, 7, 8 y 9 de septiembre de 2016, Madrid. Comité Español de
Automática, Pp. 755-761
© Comité Español de Automática (CEA-IFAC)
OBTENCIÓN DEL MODELO DINÁMICO SIMBÓLICO DE
ROBOTS RAMIFICADOS UTILIZANDO GRUPOS DE LIE Y
GRAFOS
J.A. Escalera, F.J. Abu-Dakka, P.J. Alhama y M. Abderrahim
Robotics Lab, Universidad Carlos III de Madrid,
{jescaler,fabudakk,palhama,mohamed}@ing.uc3m.es
Resumen
En este artı́culo se presenta una formulación matricial simbólica para el modelado dinámico de robots ramificados, los cuales están compuestos por
varias cadenas cinemáticas lineales abiertas. El
método propuesto utiliza la mecánica geométrica
basada en la teorı́a de Screws y grupos de Lie para derivar la ecuación de movimiento de NewtonEuler geométrica. La formulación es válida para cualquier robot formado por cuerpos rı́gidos
acoplados mediante juntas de un grado de libertad (por tanto, rotacionales y/o prismáticas) sin
formar cadenas cinemáticas cerradas. Bajo estas
condiciones, estos robots pueden ser representados
en forma única como un grafo de tipo árbol dirigido. Finalmente combinando la teorı́a de grafos
con la mecánica geométrica se obtiene el modelo
dinámico complete de robots ramificados. Además,
la formulación propuesta presenta los parámetros
intrı́nsecos del robot explı́citamente en términos
aislados. De este modo, la ecuación resultante se
puede utilizar en algoritmos tales como identificación, simulación y control.
Palabras clave: Modelado dinámico, robots
ramificados, grupos de Lie, teorı́a de grafos.
1.
INTRODUCCIÓN
En general, un sistema robótico se puede considerar como un conjunto articulado de cuerpos
rı́gidos. Por esta razón, su modelo dinámico puede obtenerse mediante la ley de Newton-Euler o
aplicando la ecuación de movimiento de Lagrange. Sin embargo, estas ecuaciones pueden volverse
extremadamente complejas con sistemas de muchos grados de libertad (GDL). Esta complejidad
ha exigido un esfuerzo a los cientı́ficos para buscar
formas de reducir la complejidad de computación
de estas ecuaciones [1]. El incremento en la complejidad de los sistemas robóticos y los métodos
para la planificación de trayectorias y control está
influyendo en la búsqueda de formulaciones de la
dinámica más sistemáticas y compactas. En esta
lı́nea, el modelado geométrico ha sido empleado
satisfactoriamente en robótica (tanto cinemática
como dinámica) [8, 14, 15, 17, 19]. Featherstone,
en 1984, propuso en su tesis una formulación espacial basada en vectores espaciales de 6 dimensiones que combinaban el aspecto lineal y angular de
la cinemática y dinámica de sistemas articulados
de cuerpos rı́gidos [7]. Posteriormente, describió
la relación de la fuerza espacial y la aceleración
de un cuerpo rı́gido del sistema usando la matriz
de inercia de ese cuerpo, considerando la dinámica del sistema articulado [8]. Además, propuso un
algoritmo recursivo de complejidad O(n) para resolver el problema dinámico directo en el espacio
articular para robots ramificados. Featherstone en
su libro, deriva parcialmente un modelo simbólico
matricial.
Otras alternativas usan los conceptos de grupos de Lie y geometrı́a diferencial para derivar
las ecuaciones dinámicas de sistemas robóticos
[4, 15, 17, 19]. Esta técnica puede aplicarse a sistemas robóticos ya que el grupo Euclidiano especial
SE(3), usado en su modelado, reúne las condiciones de un grupo de Lie. Por tanto, la dinámica de
robots puede expresarse aplicando propiedades de
los grupos de Lie y su estructura algebraica asociada [4, 12]. Park et al, propusieron un algoritmo recursivo para formular la dinámica de robots
usando grupos de Lie [14]. Un año después, éstos
extendieron su algoritmo para expresar la ecuación de movimiento de Lagrange en términos de
grupos de Lie [15].
Duindam y Stramigioli combinaron la juntas Euclidianas y otras juntas más generales, i.e. juntas
que pueden ser descritas por el grupo SE(3) o
uno de sus subgrupos [6] en un método más general y basado en el Lagrangiano. Posteriormente, se
propone una extensión de este método para modelar sistemas móviles con manipulador [10]. La
consiguiente derivación explı́cita de está ecuación
dinámica para diferentes espacios de configuraciones, muestra que cuando se reduce a la dinámica
de un único cuerpo, ésta es equivalente a la formulación clásica para cuerpos rı́gidos, concretamente
a las ecuaciones de movimiento de Newton-Euler
y Lagrange [9].
La principal contribución de este artı́culo es la derivación simbólica de la ecuación de movimiento
Cu
ad
r
o1
:N
o
ta
c
iónd
elo
se
l
em
en
to
sbá
s
i
co
sd
e
l
m
od
e
l
ad
og
e
om
é
t
r
i
co
.
Ψi S
i
s
t
emad
ecoo
rd
en
ada
sfi
joa
lcu
e
rpoi
.
Hi
M
a
t
r
i
zd
e
t
r
a
n
s
f
o
rm
a
c
i
ó
n
h
om
o
g
é
n
e
ad
e
l
j
s
i
s
t
ema Ψ
onr
e
sp
e
c
toa
ls
i
s
t
ema Ψ
.
jc
i
,
j
si
S
c
r
e
wr
e
l
a
t
i
v
od
e
l
c
u
e
r
p
o
kc
o
n
r
e
s
p
e
c
to
k
a
lcu
e
rpojexp
r
e
sadoene
ls
i
s
t
ema Ψ
.
i
i
,
j
sk
Co
s
r
ew d
elafu
e
r
zaqu
ee
lcu
e
rpok
e
j
e
r
c
esob
r
ee
lcu
e
rpojexp
r
e
sadoen Ψ
.
i
i
,
j
t
T
w
i
s
t
r
e
l
a
t
i
v
od
e
l
c
u
e
r
p
o
kc
o
n
r
e
s
p
e
c
t
o
k
a
lcu
e
rpojexp
r
e
sadoene
ls
i
s
t
ema Ψ
.
i
,
j
wi
W
r
e
n
c
h
e
j
e
r
c
i
d
op
o
r
e
l
c
u
e
r
p
o
ks
o
b
r
e
e
l
k
cu
e
rpojexp
r
e
sadoene
ls
i
s
t
ema Ψ
.
i
wi,j W
r
en
ch n
e
toe
j
e
r
c
idosob
r
ee
lcu
e
rpoj
yexp
r
e
sadaene
ls
i
s
t
ema Ψi.
,
j
rk
V
e
c
t
o
rd
e
sd
ee
lpun
toja
lpun
toi
i
exp
r
e
sadoene
ls
i
s
t
ema Ψ
.
k
x̃ M
a
t
r
i
zan
t
i
s
im
é
t
r
i
cad
e
lv
e
c
to
r x.
d
eN
ew
t
on
-Eu
l
e
rd
e
sd
elap
e
r
sp
e
c
t
iv
ag
eom
é
t
r
i
c
a
.P
a
r
at
a
lfin
,s
ehanu
t
i
l
i
zadola
st
eo
r
ı́a
sd
e
S
c
r
ew,d
eL
i
eyd
eG
ra
f
o
s
.E
s
táfo
rmu
la
c
ióne
s
v
á
l
id
ap
a
r
acua
lqu
i
e
rrobo
tfo
rmadopo
rcu
e
rpo
s
r
ı́
g
id
o
sycuy
a
sjun
ta
st
i
en
enun GDL(
ro
ta
c
iona
l
op
r
i
sm
á
t
i
c
a
)
. Ad
emá
s
,é
s
to
sd
eb
ene
s
ta
rfo
rma
d
o
spo
rc
ad
ena
sc
in
emá
t
i
ca
sab
i
e
r
ta
sd
eta
lfo
rma
qu
ee
lr
obo
tadm
i
taunar
ep
r
e
s
en
ta
c
ión m
ed
ian
t
e
ung
r
a
f
od
eá
rbo
ld
i
r
ig
ido
.En
t
r
ee
s
t
et
ipod
ero
bo
t
ss
een
cu
en
t
ran
,po
re
j
emp
lo
, mano
srobó
t
i
ca
s
yhum
an
o
id
e
s
.Ad
emá
s
,lafo
rmu
la
c
iónp
ropu
e
s
ta
d
e
j
aenf
o
rm
aexp
l
ı́
c
i
talo
spa
rám
e
t
ro
sin
t
r
ı́n
s
e
co
s
d
e
lr
obo
t
,l
ocua
le
sd
e
s
eab
l
epa
rasuap
l
i
ca
c
iónen
a
l
g
o
r
i
tm
o
sd
eid
en
t
ifi
ca
c
ión
,s
imu
la
c
ión
,s
ı́n
t
e
s
i
sy
c
on
t
r
o
l
.
2
. CONCEPTOS Y NOTAC
IÓN
DEL MODELADO
GEOMÉTR
ICO
E
s
t
as
e
c
c
i
ónp
r
e
s
en
taunab
r
ev
ein
t
rodu
c
c
iónd
e
l
o
se
l
em
en
t
o
sd
e
l mod
e
ladog
eom
é
t
r
i
coap
l
i
cad
oa
r
obo
t
s
.N
os
ep
r
e
t
end
ep
ropo
r
c
iona
runaexp
l
i
ca
c
i
ónd
e
t
a
l
l
adad
e
lt
emas
inoc
la
r
ifi
ca
rlano
ta
c
ión
emp
l
e
ad
aene
s
t
ea
r
t
ı́
cu
lo
,lacua
ls
e mu
e
s
t
raen
l
at
ab
l
a1
.P
a
rauncomp
l
e
toen
t
end
im
i
en
tod
elo
s
c
on
c
ep
t
o
sexpu
e
s
to
sene
s
tays
igu
i
en
t
e
ss
e
c
c
ion
e
s
e
ll
e
c
t
o
rpu
ed
er
ev
i
sa
r[4
,17
]
.
L
ab
a
s
ed
e
l mod
e
ladog
eom
é
t
r
i
cos
efund
am
en
t
aend
o
st
e
o
r
ema
sdua
l
e
s
:e
lt
eo
r
emad
eCha
s
l
e
s
ye
lt
e
o
r
em
ad
ePo
in
so
t [2
]
.E
lp
r
im
e
roenun
c
ia
qu
ecu
a
lqu
i
e
rmov
im
i
en
toen
t
r
edo
scu
e
rpo
spu
ed
e
d
e
s
c
r
ib
i
r
s
em
ed
ian
t
eunaro
ta
c
ións
egu
idapo
runa
c
r
ew
.E
s
t
et
eo
t
r
a
s
l
a
c
i
óns
ob
r
eun m
i
smoe
j
eos
r
em
aadm
i
t
eo
t
rain
t
e
rp
r
e
ta
c
iónha
c
i
endot
end
e
r
ac
e
r
od
e
sp
l
a
zam
i
en
toyt
i
empo
.D
ee
s
ta man
e
ra
s
eob
t
i
en
elav
e
lo
c
idadin
s
tan
tán
eag
en
e
ra
l
i
zada
r
e
la
t
iv
a
,otw
i
s
t
,en
t
r
elo
scu
e
rpo
s
.
M
i
en
t
ra
squ
ee
ls
egundo
,dua
ld
e
lp
r
im
e
ro
,enun
c
iaqu
ecadas
i
s
t
emad
efu
e
r
za
sap
l
i
cadoauncu
e
r
poe
sequ
iv
a
l
en
t
eaunafu
e
r
zayunpa
rap
l
i
cado
s
sob
r
ee
lm
i
smoe
j
eoc
o
s
c
r
ew
.E
s
tafu
e
r
zag
en
e
ra
l
i
zadar
e
c
ib
ee
lnomb
r
ed
ew
r
en
ch.
D
elap
r
im
e
rain
t
e
rp
r
e
ta
c
iónd
e
lt
eo
r
emad
eCha
s
l
e
ssu
rg
eunain
t
e
r
e
san
t
eequ
iv
a
l
en
c
iaen
t
r
ed
e
s
c
r
ew
sy ma
p
la
zam
i
en
to
sexp
r
e
sado
sm
ed
ian
t
es
t
r
i
c
e
shomog
én
ea
sH ∈SE(3
)[17
](
fo
rma
lm
en
t
e
e
ld
e
sp
la
zam
i
en
toen
t
r
edo
ss
i
s
t
ema
sd
er
e
f
e
r
en
c
ia
fi
jo
scad
aunoauncu
e
rpo
)
.D
eigua
l man
e
ras
e
pu
ed
eob
t
en
e
runar
e
la
c
iónen
t
r
etw
i
s
t
syma
t
r
i
c
e
s
homog
én
ea
sd
elas
egundain
t
e
rp
r
e
ta
c
ión
.E
s
tar
e
la
c
ións
econ
c
r
e
ta m
ed
ian
t
ee
lá
lg
eb
rad
eL
i
e
.E
l
)e
sun
g
rupoEu
c
l
id
ianoe
sp
e
c
ia
ld
ema
t
r
i
c
e
sSE(3
g
rupod
eL
i
ecuy
oe
spa
c
iotang
en
t
eenlaid
en
t
i
dadt
i
en
ee
s
t
ru
c
tu
rad
eá
lg
eb
rad
eL
i
eys
ed
en
o
ta
s
e
(3
)
.Comos
e mu
e
s
t
raen[17
]
,lo
stw
i
s
t
ssone
l
e
m
en
to
sd
es
e
(3
)
.P
o
rtan
to
,lad
e
r
iv
adat
empo
ra
l
d
es
c
r
ew
se
sequ
iv
a
l
en
t
ealad
e
r
iv
adat
empo
ra
l
˙ eta
d
e ma
t
r
i
c
e
shomog
én
ea
sH.D
l man
e
raqu
es
e
e
s
tab
l
e
c
el
as
igu
i
en
t
er
e
la
c
iónen
t
r
ee
s
to
scampo
s
i
,
j ˙
j
,
j j
˙j=Hjt̃
v
e
c
to
r
ia
l
e
s
:H
Hj
i
ii
i= t̃
i Hi .
i
s
t
syw
r
en
ch
e
ssone
l
em
en
to
sdua
l
e
s
,lo
s
Comotw
w
r
en
ch
e
sp
e
r
t
en
e
c
ena
le
spa
c
iodua
ld
e
lá
lg
eb
rad
e
∗
L
i
es
e
(3
)d
eno
tados
e
(3
)
.Ladua
l
idadp
rov
i
en
ed
e
la m
e
cán
i
caypon
ed
e man
ifi
e
s
toqu
ee
lp
rodu
c
to
dua
len
t
r
eunw
r
en
chwk,ia
c
tuandoenuncu
e
rpoi
k
,0
ysutw
i
s
tab
so
lu
tot
,
e
s
l
ap
o
t
e
n
c
i
a
i
n
s
t
a
n
t
án
ea
i
d
e
sa
r
ro
l
ladopo
re
lcu
e
rpo
.Enfo
rma ma
t
emá
t
i
ca
k
,0
k
,0
k
,
iT
s
et
i
en
e
:wk,i(
t
·
t
o
t
en
c
ia.
i )=w
i =p
2
.1
. CAMB
IO DES
ISTEMAS DE
REFERENC
IA
Tw
i
s
t
syw
r
en
ch
e
ssonv
e
c
to
r
e
s/
co
-v
e
c
to
r
e
sd
esu
s
co
r
r
e
spond
i
en
t
e
sá
lg
eb
ra
sd
eL
i
e
. Comota
l
,pu
e
d
ensuma
r
s
eensuco
r
r
e
spond
i
en
t
ee
spa
c
iov
e
c
to
r
ia
ls
i
emp
r
equ
ee
s
tánexp
r
e
sado
sene
lm
i
smo
s
i
s
t
emad
er
e
f
e
r
en
c
ia
.
P
a
racamb
ia
re
ls
i
s
t
emad
er
e
f
e
r
en
c
iaene
lqu
e
i
s
t
syw
r
en
ch
e
ss
eu
t
i
l
i
za[18
]
:
e
s
tánexp
r
e
sado
stw
l
,
j
k
,
j
dHkl ·
t
t
i =A
i ,
(1
)
,
j
,
j
wl
dT
wk
i =A
i ,
Hk ·
(2
)
l
Rl
k
l
,
l
r̃k ·
Rl
k
0
e
slar
ep
r
e
s
en
Rl
k
ta
c
ión ma
t
r
i
c
ia
ld
e
lad
jun
tod
elacon
juga
c
iónpa
l
,
l
rama
t
r
i
c
e
shomog
én
ea
sHl
Rl
r
u
et
ran
s
fo
r
k(
k,
k )q
manla
scoo
rd
enada
sd
e
ls
i
s
t
emaΨk a
ls
i
s
t
emaΨl.
dond
eAdHkl =
Lad
e
r
iv
adat
empo
ra
ld
ee
s
t
ead
jun
toe
sune
l
e
m
en
toc
lav
eenla
se
cua
c
ion
e
sd
ela
ss
igu
i
en
t
e
ss
e
c
-
c
i
on
e
s
.
d
Ad j =AdHjadti,j =adtj,jAdHj
i
i
i
i
d
t Hi
(3
)
k
,
j
ω̃i
k
,
j
ṽi
0
e
slar
ep
r
e
s
en
k
,
j
ω̃i
k
,
j
t
a
c
i
ón m
a
t
r
i
c
ia
ld
e
l ad
jun
to d
e
ltw
i
s
tt
=
i
k
,
j k
,
j
ωi ,v
.
(
i )
d
ond
eadtk,j =
i
3
. REPRESENTAC
IÓN DE
ROBOTS ENFORMA DE
GRAFOS
Unr
obo
tpu
ed
econ
s
id
e
ra
r
s
ecomouncon
jun
to
d
ecu
e
rpo
sr
ı́g
ido
sa
cop
lado
sm
ed
ian
t
epa
r
e
sc
i
n
em
á
t
i
c
o
squ
ep
e
rm
i
t
ene
l mov
im
i
en
tor
e
la
t
iv
o
en
t
r
esu
sp
a
r
t
e
s
. Ad
emá
s
,lo
ss
i
s
t
ema
sd
ecu
e
r
por
ı́
g
id
o
sa
r
t
i
cu
lado
sadm
i
t
enunr
ep
r
e
s
en
ta
c
ión
ab
s
t
r
a
c
t
aenfo
rmad
eg
ra
fo
.P
a
raob
t
en
e
rta
lr
e
p
r
e
s
en
t
a
c
i
ón
,cadacu
e
rpor
ı́g
idod
e
lrobo
ts
er
e
p
r
e
s
en
t
apo
runv
é
r
t
i
c
eyacadapa
rc
in
emá
t
i
co
s
el
ea
s
o
c
i
auna
r
co[5
]
.Ad
i
c
iona
lm
en
t
e
,lo
sa
r
co
s
pu
ed
ens
e
rd
i
r
ig
ido
s
.E
s
t
et
ipod
eg
ra
fo
ss
ed
e
n
om
in
ag
r
a
foc
in
emá
t
i
c
o
.T
a
l
e
sr
ep
r
e
s
en
ta
c
ion
e
s
d
er
obo
t
scomog
ra
fo
sc
in
emá
t
i
co
ssone
lv
ı́n
cu
lo
ad
e
cu
ad
op
a
raap
l
i
ca
rt
é
cn
i
ca
sd
et
eo
r
ı́ad
eG
ra
fo
s
[
1
1
]a
lm
od
e
ladod
erobo
t
s
.
ye
lcon
jun
tod
etodo
slo
sa
r
co
spo
rE.P
o
rtan
to
G=G(
V
,E)
.
Unap
rop
i
edadimpo
r
tan
t
ed
elo
sg
ra
fo
se
squ
e
cua
lqu
i
e
rg
ra
foe
si
somó
rfi
coaunama
t
r
i
z
.Haydo
s
r
ep
r
e
s
en
ta
c
ion
e
s ma
t
r
i
c
ia
l
e
sd
eung
ra
fo
:lama
t
r
i
z
t
r
i
zd
ein
c
id
en
c
ia.Lama
t
r
i
z
d
ead
ya
c
en
c
iaylama
d
ein
c
id
en
c
iat
i
en
ed
im
en
s
iónn×m,dond
ene
s
e
lnúm
e
rod
ev
é
r
t
i
c
e
sym e
se
lnúm
e
rod
ea
r
co
s
.
P
a
racon
s
t
ru
i
re
s
ta ma
t
r
i
z
,lo
sa
r
co
ss
eco
lo
canen
co
lumna
sylo
sv
é
r
t
i
c
e
senfi
la
s
.Cadaen
t
r
adaai,j
s
e
rá−1s
ie
la
r
cod
elaj
é
s
imaco
lumnad
e
jae
l
v
é
r
t
i
c
ed
elai
é
s
imafi
la
,1s
ie
la
r
cod
elaj
é
s
ima
co
lumnaen
t
raa
lv
é
r
t
i
c
ed
elai
é
s
imafi
lay0en
o
t
roca
so[11
]
.
Lafo
rmu
la
c
iónqu
es
eexp
l
i
ca má
sad
e
lan
t
eu
t
i
l
i
zauna ma
t
r
i
zd
ein
c
id
en
c
ia mod
ifi
cada
.P
r
im
e
ram
en
t
e
,unod
elo
sv
é
r
t
i
c
e
sd
e
lg
ra
fos
e ma
r
ca
comor
e
f
e
r
en
c
iain
e
r
c
ia
l
.P
a
rad
i
s
t
ingu
i
r
lo
,sue
t
i
qu
e
tas
esub
ray
a
.P
o
s
t
e
r
io
rm
en
t
es
ee
l
im
inad
ela
ma
t
r
i
zd
ein
c
id
en
c
ialafi
laco
r
r
e
spond
i
en
t
eae
s
t
e
v
é
r
t
i
c
e
.Ene
s
ta ma
t
r
i
zd
ein
c
id
en
c
ia mod
ifi
cada
,
cadaen
t
radas
et
ran
s
fo
rmad
eune
s
ca
la
rauna
ma
t
r
i
z6×6
.E
lcamb
iocon
s
i
s
t
eensub
s
t
i
tu
i
rca
da±1po
rla ma
t
r
i
zun
idad±I6 ycada0po
rla
ma
t
r
i
znu
la 06. E
s
tama
t
r
i
zd
ein
c
id
en
c
ia mo
d
i
fi
c
ada(M
IM
)s
er
ep
r
e
s
en
tacomoB.Lafigu
ra1
mu
e
s
t
raune
j
emp
lod
ecomoung
ra
fod
i
r
i
g
idos
e
co
lo
caenfo
rmad
eM
IM
.
3
.1
. PARES C
INEMÁT
ICOS Y
RELAC
IONES DELOS T W
ISTS
DEL ROBOT
F
i
gu
r
a1
:(
I
zqu
i
e
rda
)R
ep
r
e
s
en
ta
c
ioncomog
ra
fo
d
eunr
obo
tcompu
e
s
topo
rs
e
i
scu
e
rpo
sr
ı́g
ido
s
(v
é
r
t
i
c
e
s
)yc
in
cojun
ta
s(a
r
co
s
)
. Cadaa
r
coe
s
tá
ad
em
á
sd
i
r
i
g
id
oloqu
eind
i
caun
ar
e
la
c
ión“
r
e
la
t
i
v
oa
”en
t
r
el
o
sv
é
r
t
i
c
e
squ
eun
e
.(D
e
r
e
cha
)M
a
t
r
i
z
d
ein
c
id
en
c
i
a mod
ifi
cadad
e
lg
ra
foconlafi
lad
e
l
v
é
r
t
i
c
ed
er
e
f
e
r
en
c
iag
loba
le
l
im
inada
,qu
eene
s
t
e
c
a
s
oe
se
lv
é
r
t
i
c
ec
e
ro(
sub
ray
ado
)
.
L
an
o
t
a
c
i
ónd
eg
ra
fo
ss
igu
ela
ss
igu
i
en
t
e
sr
eg
la
s
.
C
ad
av
é
r
t
i
c
et
i
en
eunún
i
coid
en
t
i
cado
riqu
eid
en
.
D
e
b
i
d
oaq
u
e
l
o
sa
r
c
o
s
t
ifi
c
ae
lv
é
r
t
i
c
ecomov
i
s
i
emp
r
ev
in
cu
l
ando
sv
é
r
t
i
c
e
s
,suid
en
t
ifi
cado
re
s
tá
c
ompu
e
s
t
opo
rlo
sı́nd
i
c
e
sd
elo
sv
é
r
t
i
c
e
sady
a
c
en
t
e
s
,i
.
e
.e
d
e
fi
n
e
e
l
a
r
c
o
q
u
e
v
ad
e
l
v
é
r
t
i
c
ev
a
l
v
i
j
i
j.
Ad
em
á
s
,eng
ra
fo
sd
i
r
ig
id
o
s
,e
s
tano
ta
c
ión mu
e
s
t
r
aqu
ee
la
r
cod
e
jav
n
t
raenv
afigu
ra1
iye
j.L
mu
e
s
t
r
aune
j
emp
lo
.A
s
im
i
smo
,dadoung
ra
fo G,
e
lc
on
jun
t
od
etodo
slo
sv
é
r
t
i
c
e
ss
ed
eno
tapo
rV
Comos
ehaind
i
cadoenlas
e
c
c
iónan
t
e
r
io
r
,cada
pa
rc
in
emá
t
i
col
l
ev
aa
so
c
iadouna
r
cod
e
lg
ra
fo
.
Unpa
rc
in
emá
t
i
coa
cop
lado
se
s
labon
e
sd
e
lrobo
t
d
eta
l man
e
raqu
ep
e
rm
i
t
eun mov
im
i
en
tor
e
la
t
i
v
oen
t
r
ee
l
la
s
.D
eb
idoaqu
ee
lmov
im
i
en
tor
e
la
t
iv
o
en
t
r
edo
scu
e
rpo
spu
ed
es
e
rd
e
s
c
r
i
topo
runs
c
r
ew
∈R6,l
acomb
ina
c
iónd
es
e
i
ss
c
r
ew
sl
in
ea
lm
en
t
e
ind
ep
end
i
en
t
e
sd
e
s
c
r
ib
ecua
lqu
i
e
r mov
im
i
en
tor
e
la
t
iv
oene
le
spa
c
io
.S
e
l
e
c
c
ionandounsub
con
jun
tod
ee
s
t
es
i
s
t
emas
eob
t
i
en
eunsub
e
spa
c
ioqu
e
d
e
s
c
r
ib
esó
loaqu
e
l
lo
s mov
im
i
en
tor
e
la
t
iv
o
squ
e
sonp
e
rm
i
t
ido
spo
rlajun
ta
.E
lsub
e
spa
c
iocom
p
l
em
en
ta
r
iocon
t
i
en
elo
s mov
im
i
en
tor
e
la
t
iv
o
squ
e
sonimp
ed
ido
spo
rlajun
ta
.
Ex
i
s
t
endo
sg
rupo
sd
epa
r
e
sc
in
emá
t
i
co
s
:lo
sin
f
e
r
io
r
e
sylo
ssup
e
r
io
r
e
s[16
]
.Ene
la
l
can
c
ed
ee
s
t
e
a
r
t
ı́
cu
losó
los
et
ra
tanlo
spa
r
e
sc
in
emá
t
i
co
sin
f
e
r
io
r
e
s
.E
s
t
eg
rupo
,asuv
e
z
,con
t
i
en
es
e
i
st
ipo
sd
e
jun
ta
s
.S
inemba
rgo
,só
lodo
sd
ee
l
lo
spu
ed
encon
s
id
e
ra
r
s
ecomoe
l
em
en
ta
l
e
sy
aqu
ecomb
inándo
lo
s
s
econ
s
igu
en mov
im
i
en
to
sr
e
la
t
iv
o
sid
én
t
i
co
salo
s
d
elo
so
t
ro
spa
r
e
s
.E
s
to
sdo
se
l
em
en
t
o
ssonla
sjun
ta
sro
ta
c
iona
lyp
r
i
smá
t
i
ca
. Amba
ssonjun
ta
sd
e
un GDLd
eta
l man
e
raqu
esusub
e
spa
c
iod
e mo
-
v
im
i
en
t
or
e
l
a
t
iv
oqu
ed
ad
efin
idopo
runs
i
s
t
ema
d
eunún
i
c
os
c
r
ew
.P
o
rtan
to
,cu
a
lqu
i
e
ra
r
coe
i
,
j
,
j
d
e
lg
r
a
f
ot
i
en
ea
so
c
iadouns
c
r
ewsj
b
s
é
rv
e
s
e
i .O
qu
es
ie
ls
c
r
ewe
s
táexp
r
e
sadoconr
e
sp
e
c
toaun
s
i
s
t
em
ad
er
e
f
e
r
en
c
iafi
joaa
lgunod
elo
scu
e
rpo
s
ady
a
c
en
t
ea
la
r
co
,d
i
chos
c
r
ewe
scon
s
tan
t
e
.
O
t
r
av
en
t
a
j
ad
eu
sa
rjun
ta
sd
eun GDLe
squ
ela
m
a
gn
i
tudd
e
ls
c
r
ewqu
edad
efin
idapo
runún
i
co
p
a
r
ám
e
t
r
o
.P
o
rcon
s
igu
i
en
t
e
,pa
raca
l
cu
la
re
ld
e
s
p
l
a
z
am
i
en
t
or
e
la
t
iv
oolav
e
lo
c
idadr
e
la
t
iv
aen
t
r
e
l
o
scu
e
rpo
sun
ido
spo
rlajun
ta
,ún
i
cam
en
t
es
en
e
c
e
s
i
t
asupo
s
i
c
ióng
en
e
ra
l
i
zadaqosuv
e
lo
c
idad
g
en
e
r
a
l
i
z
ad
aq̇r
e
sp
e
c
t
iv
am
en
t
e
.
P
o
rt
an
t
o
,d
adouns
i
s
t
emaa
r
t
i
cu
ladod
ecu
e
rpo
s
r
ı́
g
id
o
se
lcu
a
ladm
i
t
eunr
ep
r
e
s
en
ta
c
ióneng
ra
f
o
V
,E)∈
e
t
iqu
e
t
ad
oenfo
rmad
eá
rbo
ld
i
r
ig
idoG(
Rn×n−1,l
ae
cua
c
ión d
e mov
im
i
en
tos
imbó
l
i
ca
g
e
om
é
t
r
i
c
apu
ed
efo
rmu
la
r
s
eenfo
rmacompa
c
ta
d
efin
i
end
ol
o
ss
igu
i
en
t
e
se
l
em
en
to
s
:
Defin
ic
ión:Una ma
t
r
i
zd
iagona
lab
loqu
e
sSb
A∈
6n×n
R
qu
ec
on
t
i
en
etodo
slo
ss
c
r
ew
sa
so
c
iado
scon
c
ad
aa
r
c
od
eG(
V
,E)yfo
rmadacomos
igu
e
j
,
j
Sb
iag s
A =d
i
∀e
i
,
j∈E
(4
)
s
ed
en
om
in
as
c
r
ewc
on
jun
tod
ela
sjun
ta
sye
s
c
on
s
t
an
t
e
.
Defin
ic
ión: Una ma
t
r
i
zd
iagona
lab
loqu
e
s
AdHB0 ∈ R6n×6n fo
rmada po
rla
s ma
t
r
i
c
e
sd
e
t
r
an
s
f
o
rm
a
c
i
ónd
e
ls
i
s
t
emalo
ca
la
lin
e
r
c
ia
ld
eca
d
atw
i
s
t
sd
elo
sv
é
r
t
i
c
e
sd
eG(
V
,E)s
ed
enom
ina
t
r
an
s
fo
rma
c
iónc
on
jun
tad
ec
o
o
r
d
enada
scuy
afo
r
m
ae
s
:
AdHB0 =d
i
ag AdHi0
∀v
} (5
)
i∈{V−0
P
a
r
aob
t
en
e
rlat
ran
s
fo
rma
c
iónh
omog
én
eaen
t
r
e
s
i
s
t
em
a
sd
ecoo
rd
enado
sd
ecu
e
rpo
sady
a
c
en
t
e
sa
l
am
i
sm
ajun
tae
eu
t
i
l
i
zae
lap
l
i
ca
c
iónexpo
i
,
js
n
en
c
i
a
l[
1
3
]s
igu
i
en
t
e
:
,j
s̃i
qj
i ·
j
Hj
q
0
)·
e
j)=Hi(
i(
,j
s̃j
qj
i ·
=e
·
Hj
0
) (6
)
i(
L
as
e
cu
en
c
i
acomp
l
e
tad
et
ran
s
f
o
rma
c
ion
e
sd
e
s
d
ee
ls
i
s
t
em
acoo
rd
enadod
er
e
f
e
r
en
c
ia Ψ0 ha
s
ta
e
ls
i
s
t
em
aΨ
eob
t
i
en
em
ed
ian
t
ee
lp
rodu
c
tod
e
js
expon
en
c
i
a
l
e
s[3
]
.
0
6n×1
Defin
ic
ión:Unv
e
c
to
rt
qu
eag
rupa
K ∈R
l
o
stw
i
s
t
sa
s
o
c
iado
sconlo
sa
r
co
sd
e
lg
ra
f
oG(
V
,E)
i
s
tc
on
jun
toa
r
t
i
cu
la
rysa
t
i
s
fa
c
ela
s
ed
en
om
in
atw
s
i
gu
i
en
t
ei
gu
a
ldad
:
0,
j
0
o
l t
t
=AdHB0 ·
Sb
q̇
A·
K =c
i
∀e
,
i
,
j∈E
(7
)
d
ond
eq̇=c
o
l
(
˙
q
)e
se
lv
e
c
t
o
rc
o
l
um
n
ad
e
t
o
d
a
s
j
l
a
sv
e
l
o
c
id
ad
e
sd
ela
sjun
ta
s
.
0
6n×1
Defin
ic
ión:Unv
e
c
to
rt
qu
eag
rupalo
s
B ∈R
tw
i
s
t
sr
e
la
t
iv
o
sen
t
r
ecadav
é
r
t
i
c
eye
lv
é
r
t
i
c
ed
e
r
e
f
e
r
en
c
iad
eG(
V
,E)(
con
c
r
e
tam
en
t
esus
i
s
t
ema
coo
rd
enadoa
so
c
iad
o
)s
ed
enom
inatw
i
s
tc
on
jun
to
s
t
ru
c
tu
rae
slas
igu
i
en
t
e
:
a
b
so
lu
toysue
0,
0
0
o
l t
t
B =c
j
∀v
}
j∈V−{0
(8
)
Ex
i
s
t
eunar
e
la
c
iónfundam
en
ta
len
t
r
ee
ltw
i
s
tcon
jun
toab
so
lu
to
,e
ltw
i
s
tcon
jun
toa
r
t
i
cu
la
ryla
M
IMqu
ee
s[18
]
:
T 0
0
t
t
K =−B ·
B,
(9
)
P
a
racomp
r
end
e
rd
i
ch
ar
e
la
c
ión(9
)
,s
ed
eb
eob
s
e
rv
a
rqu
ecadafi
lad
e−BT r
e
la
c
ionasuco
r
r
e
s
pond
i
en
t
ejun
taconlo
sdo
scu
e
rpo
squ
een
la
za
.
0
P
o
rtan
to
,cadafi
la(
i
.
ee
u
l
t
ip
l
i
cadapo
rt
i
,
j) m
B
dacomor
e
su
l
tadolasumad
elo
sco
r
r
e
spond
i
en
0,
0
0,
0
yt
.
D
e
b
i
d
oa
l
ar
e
l
a
c
i
ó
n
d
e
t
e
se
l
em
en
to
st
i
j
o
rd
enimpu
e
s
tapo
rlo
sa
r
co
sye
ls
igno m
eno
sd
e
0,
0
0,
0
(9
)lasumae
st
efo
rmaequ
iv
a
l
en
t
e
i −t
j od
0,
0
0,
0
0,
i
0,
j
t
u
er
e
su
l
taent
omos
e
r
ı́a
i − t
i +t
j .Q
i c
d
e
s
eab
l
epa
rae
i
,
j.
F
ina
lm
en
t
e
,e
lco
ro
la
r
io4
.2
.6d
e[11
]p
ru
ebala
ex
i
s
t
en
c
iad
eB−1,conloqu
es
ev
e
r
ifi
calae
cua
c
iónr
e
c
ı́p
ro
cas
i
gu
i
en
t
e
:
−T 0
0
0
t
)
·
t
t
AdHB0 ·
Sb
q̇(10
A·
B =−B
K =C·
K =C·
dond
epa
ras
imp
l
ifi
ca
rlano
ta
c
ión
,−B−T s
er
e
p
r
e
s
en
tapo
rC.
4
. OBTENC
IÓN DELAS
ECUAC
IONES DE
MOV
IM
IENTO
GEOMÉTR
ICO PARA
ROBOTS RAM
IF
ICADOS
E
s
tas
e
c
c
iónp
ropo
r
c
ionaunad
edu
c
c
ióncomp
l
e
ta
d
elae
cua
c
ión d
e mov
im
i
en
tod
eN
ew
ton
-Eu
l
e
r
d
e
sd
elap
e
r
sp
e
c
t
iv
ad
ela m
e
cán
i
cag
eom
é
t
r
i
ca
in
t
rodu
c
idaenla
ss
e
c
c
ion
e
san
t
e
r
io
r
e
s
. Lafo
r
mu
la
c
iónd
ee
s
tae
cua
c
ióne
s
táexp
r
e
sadaenfo
r
mac
e
r
radad
eta
l modoqu
epu
ed
es
e
rin
t
eg
ra
dana
tu
ra
lm
en
t
eeno
t
ra
sexp
r
e
s
ion
e
sma
t
emá
t
i
ca
s
pa
ras
e
r man
ipu
lad
as
imbó
l
i
cam
en
t
e
. Ad
emá
s
,la
fo
rmu
la
c
iónp
ropu
e
s
taexh
ib
eexp
l
ı́
c
i
tam
en
t
elo
s
pa
rám
e
t
ro
sin
t
r
ı́n
s
e
co
sd
e
lm
e
can
i
smo
.E
s
t
et
ra
ba
j
oex
t
i
end
ee
lt
raba
jo[7
]dandounafo
rmu
la
c
ións
imbó
l
i
cacomp
l
e
tad
elae
cua
c
iónd
eN
ew
ton
Eu
l
e
rye
l[15
]amp
l
iandol
aap
l
i
ca
c
iónd
elae
cua
c
iónarobo
t
sram
ifi
cado
s
.R
e
sp
e
c
toasurangod
e
ap
l
i
ca
c
ión
,é
s
t
eeng
lobacua
lqu
i
e
rrobo
tfo
rmado
po
rcu
e
rpo
sr
ı́g
ido
sun
ido
sen
t
r
es
ı́ m
ed
ian
t
ejun
ta
d
eunGDLyl
ib
r
ed
ebu
c
l
e
s
,i
.
e
.toda
sla
scad
ena
s
c
in
em
á
t
i
c
a
squ
efo
rmane
lrobo
tsonab
i
e
r
ta
s
.E
s
t
e
t
ipod
er
obo
t
st
i
en
epo
rtan
toncu
e
rpo
syn−1
jun
t
a
sd
eun GDL
. Ob
s
é
rv
e
s
equ
elacomb
in
a
c
ión
d
ejun
t
a
sr
o
t
a
c
iona
l
e
sy/op
r
i
smá
t
i
ca
sconcu
e
rpo
s
d
em
a
s
ad
e
sp
r
e
c
iab
l
edaluga
rao
t
rot
ipod
ejun
t
a
su
t
i
l
i
z
ad
a
shab
i
tua
lm
en
t
epo
rlo
srobo
t
s(
jun
ta
s
un
iv
e
r
s
a
l
e
s
,e
s
f
é
r
i
ca
s
,e
t
c
)
,po
rcon
s
igu
i
en
t
elafo
r
mu
l
a
c
i
ónp
r
opu
e
s
taaba
r
caun may
o
rnúm
e
rod
e
c
a
s
o
sd
el
o
squ
eap
r
io
r
icab
r
ı́ae
sp
e
ra
r
.
ob
t
i
en
ee
l mom
en
tog
eom
é
t
r
i
co
:
4
.1
. ECUAC
IÓN DE MOV
IM
IENTO
GEOMÉTR
ICA DE
NE WTON
-EULER
se
lw
r
en
ch n
e
toa
c
tuandosob
r
ee
l
dond
ew0,i e
cu
e
rpoiyp0,ie
se
l mom
en
tog
eom
é
t
r
i
cod
e
lcu
e
r
poi
,ambo
sexp
r
e
sado
sene
ls
i
s
t
emain
e
r
c
ia
lΨ
0.
En m
e
c
án
i
c
ac
lá
s
i
ca
,las
egundal
eyd
eN
ew
ton
enun
c
i
aqu
elafu
e
r
zan
e
tae
j
e
r
c
idasob
r
euncu
e
r
poe
si
gu
a
lalad
e
r
iv
adat
empo
ra
ld
e
l mom
en
tol
i
n
e
a
ld
et
a
lcu
e
rpo
. Aná
logam
en
t
e
,lae
cua
c
iónd
e
Eu
l
e
re
s
t
ab
l
e
c
equ
ee
l mom
en
ton
e
toap
l
i
cadoa
uncu
e
rpoe
sigua
lalad
e
r
iv
adat
empo
ra
ld
e
l mo
m
en
t
oan
gu
l
a
r
.Amba
sexp
r
e
s
ion
e
sun
id
a
sf
o
rman
l
ae
cu
a
c
i
ónd
eN
ew
ton
-Eu
l
e
r
.Ob
s
é
rv
e
s
equ
etodo
s
l
o
se
l
em
en
t
o
sinv
o
lu
c
rado
sd
eb
ene
s
ta
rexp
r
e
sado
s
enuns
i
s
t
em
acoo
rd
enadoin
e
r
c
ia
l
.
T
a
l
e
se
cu
a
c
i
on
e
s pu
ed
en ap
l
i
ca
r
s
ed
e man
e
ra
an
á
l
o
g
aenm
e
cán
i
cag
eom
é
t
r
i
capa
r
t
i
endod
e
lmo
m
en
t
og
e
om
é
t
r
i
co
.Comos
eexpon
een[17
]
,e
l mo
m
en
t
og
e
om
é
t
r
i
cor
e
su
l
tad
elaa
c
c
iónd
eunt
en
e
(3
)t
ran
s
fo
rm
ándo
lo
s
s
o
rsob
r
el
o
se
l
em
en
to
sd
es
∗
ene
l
em
en
t
o
sd
es
e
(3
)
.D
i
chot
en
so
rs
ed
enom
ina
t
en
s
o
rd
ein
e
r
c
iag
eom
é
t
r
i
co
.E
lt
en
so
re
sunop
e
r
ad
o
rm
a
t
r
i
c
ia
ls
im
é
t
r
i
coyd
efin
idopo
s
i
t
iv
ocuy
a
s
en
t
r
ad
a
ss
onlo
spa
rám
e
t
ro
sd
inám
i
co
sin
t
r
ı́n
s
e
co
s
d
e
lcu
e
rpoc
o
lo
cado
sd
e
ls
igu
i
en
t
e modo
:


2
k
,
i
k
,
i
k
,C Mi
I
−mi r̃C Mi
mir̃C Mi 
M k,i = 
,
k
,
i
−mir̃C Mi
miI3
(11
)
k
,C Mi
d
ond
eI
e
se
lt
en
so
rd
ein
e
r
c
iac
l
á
s
i
cod
e
l
cu
e
rpoiensuc
en
t
rod
ema
sa(
CMi)
,r
e
la
t
iv
oaun
s
i
s
t
em
ac
oo
rd
enadoene
lc
en
t
rod
e ma
sacone
j
e
s
p
a
r
a
l
e
l
o
sa
ls
i
s
t
emafi
joene
lcu
e
rpobodyi(Ψ
)
i,
k
,
i
mie
sl
am
a
s
ad
e
lcu
e
rpoi
,r
e
s
e
l
v
e
c
t
o
rd
e
s
d
e
C Mi
e
lo
r
i
g
end
e
ls
i
s
t
emad
e
lcu
e
rpoia
lc
en
t
rod
ema
sa
d
e
lcu
e
rpoiyI3 ∈R3×3 e
sla ma
t
r
i
zid
en
t
idad
.
Ad
em
á
s
,t
od
o
slo
se
l
em
en
to
se
s
tánexp
r
e
sado
sen
e
ls
i
s
t
em
aΨ
k.
D
eb
id
oaqu
ee
lt
en
so
rd
ein
e
r
c
iag
eom
é
t
r
i
cor
e
la
∗
c
i
on
av
e
c
t
o
r
e
sd
es
e
(3
)conco
-v
e
c
to
r
e
sd
es
e
(3
)
en
t
on
c
e
ssut
ran
s
fo
rma
c
iónen
t
r
ed
i
f
e
r
en
t
e
ss
i
s
t
e
m
a
sc
oo
rd
en
ado
sd
eb
esa
t
i
s
fa
c
e
r
:
,
i
,
i
Ml
dT
Mk
AdHlk
j =A
j ·
Hk ·
l
(12
)
C
om
os
eh
am
en
c
ionadaan
t
e
r
io
rm
en
t
e
,ap
l
i
cando
e
lt
en
s
o
rd
ein
e
r
c
iag
eom
é
t
r
i
cosob
r
euntw
i
s
ts
e
k
,0
pk,i =M k,i·t
i .
(13
)
F
ina
lm
en
t
e
,d
e
spu
é
sd
elad
efin
i
c
iónd
e
l mom
en
tog
eom
é
t
r
i
cos
eexp
r
e
salae
cua
c
iónd
eN
ew
ton
Eu
l
e
rene
ldom
in
iod
ela m
e
cán
i
cag
eom
é
t
r
i
caco
mo
:
d
w0,i= p0,i,
(14
)
d
t
A
ligua
lqu
eenlas
e
c
c
ión3
.1
,acon
t
inua
c
ións
e
d
efin
enl
o
ss
igu
i
en
t
ee
l
em
en
to
sex
t
end
ido
satodo
e
lcon
jun
to
.
Defin
ic
ión: Una ma
t
r
i
zd
iagona
lab
loqu
e
s
6n×6n
M0
fo
rmadapo
re
lt
en
so
rd
ein
e
r
c
ia
B ∈R
g
eom
é
t
r
i
coco
r
r
e
spond
i
en
t
eacadav
é
r
t
i
c
es
ed
eno
m
ina t
en
so
rc
on
jun
tod
ein
e
r
c
iacuy
acompo
s
i
c
ión
e
s
:
M0
iag M 0,i
B =d
∀v
} (15
)
i∈{V−0
6n×1
Defin
ic
ión:Unco
-v
e
c
to
rw0
qu
eag
ru
B ∈R
palo
sw
r
en
ch
e
sn
e
to
sd
ecadav
é
r
t
i
c
eexp
r
e
sado
s
ene
ls
i
s
t
emad
er
e
f
e
r
en
c
iain
e
r
c
ia
lΨ
ed
enom
i
0s
naw
r
en
chc
on
jun
toa
b
so
lu
toysue
s
t
ru
c
tu
rae
sla
s
igu
i
en
t
e
:
w0
la w0,j
B =fi
∀v
}
j∈V−{0
(16
)
S
i(13
)s
esub
s
t
i
tuy
econv
en
i
en
t
em
en
t
een(14
)y
s
eex
t
i
end
eatodoe
lrobo
tt
en
i
endoencu
en
ta(8
)
,
(15
)y(16
)s
eob
t
i
en
elae
cua
c
ióng
eom
é
t
r
i
cad
e
N
ew
ton
-Eu
l
e
rd
e
lrobo
tram
ifi
cado
:
w0
B=
d
0
M0 ·
t
,
d
t B B
(17
)
Acon
t
inua
c
ión
,sub
s
t
i
tuy
endo(10
)en(17
)yap
l
i
candolad
e
r
iv
adat
empo
ra
lr
e
su
l
ta
:
d
0
w0
C·
AdHB0 ·Sb
q̇
+
B =d
A·
tM B ·
d
0
b
0
MB ·
C·dtAdHB ·
SA ·q̇
+
+M 0
C·AdHB0 ·
Sb
q̈
.
A·
B·
(18
)
An
t
e
sd
econ
t
inua
rs
ep
r
e
c
i
saca
l
cu
la
rlad
e
r
iv
ada
t
empo
ra
ld
e
lt
en
so
rcon
jun
tod
ein
e
r
c
ia
.P
r
im
e
ro
,
cadaen
t
radas
et
ran
s
fo
rmad
e
ls
i
s
t
emain
e
r
c
ia
la
l
lo
ca
l(fi
joene
lp
rop
iocu
e
rpo
)ap
l
i
cando(12
)
.D
e
ta
l man
e
raqu
ee
lt
en
so
rcon
jun
tod
ein
e
r
c
iar
e
su
l
tan
t
equ
edacon
s
tan
t
e
. En
ton
c
e
ss
eca
l
cu
lala
d
e
r
iv
adat
empo
r
a
l
:
d
0
d
tM B
d
=d
d−T
·
Mb
Ad−1
+
B·
tA
H0
H0
B
d
Ad−T
·
Mb
d−1
.
B·
d
tA
H0
H0
B
B
B
(19
)
1 =A
N
ó
t
e
s
equ
e Ad−1
= Ad(
dHBb . Dond
e
H0
H0 )
B
B
AdHBb yM b
onaná
logo
saAdHB0 yM 0
e
sp
e
c
t
i
Bs
Br
v
am
en
t
es
a
lv
oqu
esu
se
l
em
en
to
se
s
tánexp
r
e
sado
s
enc
oo
rd
en
ada
slo
ca
l
e
sd
ecadacu
e
rpo
.
Sub
s
t
i
tuy
endo(3
)en(19
)an
t
e
r
io
r
,s
eob
t
i
en
ee
l
r
e
su
l
t
ad
ofin
a
ld
elad
e
r
iv
adat
empo
ra
ld
e
lt
en
so
r
c
on
jun
t
od
ein
e
r
c
ia
:
=ad−T
Ad−T
Mb
Ad−1
0 ·
0 ·
0+
B·
HB
t
HB
B
−T
−1
−1
b
AdH0 ·
M B ·AdH0 ·
adt0 =
B
B
B
−T
0
0
adt0 ·
M B +M B ·
ad−1
0 .
t
B
B
d
0
d
tM B
(20
)
0
M0
ad−1
·
C·
AdHB0 ·
w0
d−T
0 ·
0
B= a
B +M B ·
t
t
B
B
b
0
SA ·q̇+M B ·
C·
adt0B ·
AdHB0 ·Sb
·
+
A q̇
0
b
MB ·
C·
AdHB0 ·
SA ·
q̈
.
(21
)
MODELO D
INÁM
ICO
S
IMBÓL
ICO DE ROBOTS
RAM
IF
ICADOS
Ene
s
t
as
e
c
c
i
óns
ed
e
r
iv
alaexp
r
e
s
iónd
e
l mod
e
l
od
in
ám
i
c
oinv
e
r
soqu
eob
t
i
en
elo
sw
r
en
ch
e
s qu
e
a
c
tú
ans
ob
r
ecadaunad
ela
sjun
ta
senfun
c
iónd
e
l
m
o
v
im
i
en
t
od
e
lrobo
t(po
s
i
c
ión
,v
e
lo
c
idad
,ya
c
e
l
e
r
a
c
i
ón
)yd
elo
sw
r
en
ch
e
s ex
t
e
r
io
r
e
squ
ea
c
túan
s
ob
r
ee
lm
i
smo
.Lo
sw
r
en
ch
e
sp
r
e
s
en
t
e
sencada
jun
t
as
ed
eb
enala
sfu
e
r
za
s/pa
r
e
simpu
e
s
to
spo
r
l
ajun
t
ap
a
rar
e
s
t
r
ing
i
rc
i
e
r
to
s mov
im
i
en
to
s
,a
s
ı́
c
om
ol
a
sfu
e
r
za
s/pa
r
e
sg
en
e
rado
spo
re
la
c
tuado
r
a
s
o
c
i
ad
oacad
ajun
ta
.
P
a
r
aob
t
en
e
rta
le
cua
c
ión
,p
r
im
e
ros
ea
ı́
s
la un
e
lrobo
tys
ee
s
tab
l
e
c
ee
lba
lan
c
ed
e
cu
e
rpoid
w
r
en
ch
e
sa
c
tuandosob
r
eé
lob
t
en
i
endo
:
N
w0,i=
j=1
,
i
w0
j −
M
k=1
,k
0,
i
w0
i +we ,
(22
)
,
i
d
ond
ew0
onw
r
en
ch
e
squ
ee
lcu
e
rpoir
e
c
ib
ed
e
j s
0,k
o
t
r
o
scu
e
rpo
sjady
a
c
en
t
e
s
,wi sonw
r
en
ch
e
squ
e
e
lcu
e
rpoie
j
e
r
c
esob
r
eo
t
ro
scu
e
rpo
skun
ido
sa
,
i
onw
r
en
ch
e
sd
eb
idoaag
en
t
e
sex
t
e
rno
s
.
é
l
,yw0
e s
En
t
r
el
o
sw
r
en
ch
e
sex
t
e
r
io
r
e
se
l má
sg
en
e
ra
le
se
l
w
r
en
ch d
eb
idoalag
rav
edad
.É
s
t
e man
ifi
e
s
tasu
a
c
c
i
ónc
om
ounafu
e
r
zapu
raene
lc
en
t
rod
e ma
sa
d
e
lcu
e
rpo
,po
rl
otan
tonoe
j
e
r
c
en
ingún mom
en
t
o
.P
a
r
aob
t
en
e
rs
ev
a
lo
rs
eana
l
i
zae
ls
i
s
t
emacon
e
lw
r
en
chg
r
av
i
ta
to
r
iocomoún
i
caa
c
c
iónsob
r
eé
l
.
E
se
s
t
esupu
e
s
to
,e
ltw
i
s
timpu
e
s
topo
rlag
rav
e
d
ada
lcu
e
rpo
,exp
r
e
sadoenuns
i
s
t
emain
e
r
c
ia
l
,y
su m
om
en
t
og
eom
é
t
r
i
coson
:
0,
0
t
=
g
i
0
3
−k0
0,
i
0,
i 0,
0
v
·
t
23
)
g ,(
g → p =M
i
Sub
s
t
i
tuy
endo(17
)ene
l mom
en
tog
eom
é
t
r
i
cod
e
b
idoalag
rav
edad(23
)ysu
s
t
i
tuy
éndo
loen(20
)
r
e
su
l
tae
lw
r
en
chg
rav
i
ta
to
r
io
:
,
i
0,
0
w0
d−T
M 0,i+M 0,i·
ad−1
·
t
0,
0·
0,
0
g +
g = a
t
t
i
F
in
a
lm
en
t
e
,s
is
eap
l
i
ca(20
)en(18
)s
eob
t
i
en
e
l
aexp
r
e
s
i
ón ma
t
r
i
c
ia
ls
imbó
l
i
ca d
elae
cua
c
ión
g
e
om
é
t
r
i
c
ad
e mov
im
i
en
tod
eN
ew
ton
-Eu
l
e
r
:
4
.2
.
dond
ev
sla magn
i
tud d
ela v
e
lo
c
idadin
s
g e
tan
tán
eayk0e
se
lv
e
c
to
rd
i
r
e
c
c
iónd
elag
rav
edad
r
e
sp
e
c
toa
ls
i
s
t
emain
e
r
c
ia
l
.
i
i
d 0,
0
M 0,i·d
=M 0,i·
g
tt
i
03
−k0
g
,
(24
)
d
dond
eg= d
v
e
s
l
aa
c
e
l
e
r
a
c
i
ó
n
d
e
l
ag
r
a
v
e
d
a
d
.
t g
0,
0
0,
0
l
Pu
e
s
toqu
ee
ltw
i
s
tab
so
lu
tot
sigua
lat
g ,e
i e
i
p
r
im
e
rsumandos
eanu
l
a
.E
s
t
er
e
su
l
tadoco
in
c
id
e
cone
ldadoen[17
]
.
P
a
raex
t
end
e
rl
afo
rmu
la
c
ióna
lrobo
tcomp
l
e
tos
e
d
efin
ee
ls
igu
i
en
t
et
é
rm
ino
:
6n×1
qu
e
Defin
ic
ión:Un co
-v
e
c
to
r w0
E ∈ R
ag
rupalo
sw
r
en
ch
e
s ex
t
e
rno
sd
ecadav
é
r
t
i
c
ed
e
G(
V
,E)exp
r
e
sado
sene
ls
i
s
t
ema d
er
e
f
e
r
en
c
ia
in
e
r
c
ia
lΨ
ed
enom
inaw
r
en
chc
on
jun
toe
x
t
e
r
io
r
0s
ysue
s
t
ru
c
tu
rae
slas
igu
i
en
t
e
:
w0
la
E =fi
l
,
i
w0
el
∀v
} (25
)
j∈V−{0
Ex
t
end
i
endo(22
)y(24
)atodoe
ls
i
s
t
emaysub
s
t
i
tuy
endo(25
)s
eob
t
i
en
elaexp
r
e
s
iónd
e
l mod
e
lo
d
inám
i
cocomp
l
e
to
:
0
0
0
w0
w0
ṫ
B =B·
K +M B ·
g+wE
(26
)
0
T
T
dond
eṫ
o
l [0
−kT]
gpa
ratodov
i∈
3,
g =c
i ·
V−{0
}.Laigua
ldadw0
w0
u
rg
era
zo
B = B·
K s
nandod
e man
e
raaná
logaa(9
)cuandolo
sún
i
co
s
w
r
en
ch
e
sp
r
e
s
en
t
e
sene
ls
i
s
t
emasonlo
sd
eb
ido
s
ala
sjun
ta
s
.
5
. CONCLUS
IONES
Ene
s
t
ea
r
t
ı́
cu
lolae
cua
c
ión d
e mov
im
i
en
tod
e
N
ew
ton
-Eu
l
e
re
sob
t
en
idaene
l dom
in
io d
ela
m
e
cán
i
cag
eom
é
t
r
i
ca
. Comor
e
su
l
tados
eob
t
i
en
e
unaexp
r
e
s
iónc
e
r
radaana
l
ı́
t
i
capa
raob
t
en
e
re
l
mod
e
lod
inám
i
cod
erobo
t
sram
ifi
cado
s
.Lav
a
l
i
d
e
zd
ee
s
t
em
é
todos
eex
t
i
end
eacua
lqu
i
e
rrobo
t
fo
rmadopo
rcu
e
rpo
sr
ı́
g
ido
syen
la
zado
spo
rjun
ta
sd
e1 GDL(p
r
i
smá
t
i
ca
sy/oro
ta
c
iona
l
e
s
)cu
y
ar
ep
r
e
s
en
ta
c
ióncomog
ra
foe
suná
rbo
ld
i
r
ig
ido
.
P
o
rtan
to
,e
lrobo
tnocon
t
i
en
en
ingunacad
ena
c
in
emá
t
i
cac
e
r
rada
.A
ls
e
runaexp
r
e
s
ións
imbó
l
i
ca ma
t
r
i
c
ia
l
,é
s
tapu
ed
es
e
r man
ipu
ladacomota
l
ap
l
i
candot
é
cn
i
ca
sd
ecá
l
cu
loy/oa
lg
eb
ra
i
ca
spa
ra
comb
ina
r
la
scona
lgo
r
i
tmo
su
t
i
l
i
zado
senrobó
t
i
ca
. Ad
i
c
iona
lm
en
t
e
,laexp
r
e
s
iónr
e
su
l
tan
t
eag
ru
paexp
l
ı́
c
i
tam
en
t
elo
spa
rám
e
t
ro
sin
t
r
ı́n
s
e
co
sd
e
l
robot en forma de matrices lo cual es atractivo
en tareas de identificación. Por tanto, la ecuación simbólica propuesta proporciona una notación más completa y compacta que las actualmente existentes.
Agradecimientos
Este trabajo ha sido principalmente financiado por
el MINISTERIO DE ECONOMIA Y COMPETITIVIDAD en el proyecto con referencia RTC2014-3070-5. Además ha sido parcialmente financiado por el proyecto RoboCity2030-III-CM
(Robótica aplicada a la mejora de la calidad de vida de los ciudadanos. fase III; S2013/MIT-2748),
financiado por Programas de Actividades I+D en
la Comunidad de Madrid y cofinanciado por Fondos Estructurales de la EU.
Referencias
[1] Constantinos A. Balafoutis and Rajnikant V.
Patel. Dynamic Analysis of Robot Manipulators. Springer US, 1991.
[2] Robert Stawell Ball. A Treatise on the Theory
of Screws. Cambridge university press, 1998.
[3] Roger W Brockett. Robotic manipulators and
the product of exponentials formula. In Mathematical theory of networks and systems, pages 120–129. Springer, 1984.
[4] Francesco Bullo and Andrew D. Lewis.
Geometric Control of Mechanical Systems.
Springer-Verlag New York, 2005.
[5] FRE Crossley. The permutations of kinematic chains of eight members or less from the
graph-theoretic viewpoint. Developments in
theoretical and applied mechanics, 2:467–486,
1964.
[6] Vincent Duindam and Stefano Stramigioli.
Singularity-free dynamic equations of openchain mechanisms with general holonomic
and nonholonomic joints. Robotics, IEEE
Transactions on, 24(3):517–526, 2008.
[7] Roy Featherstone. Robot dynamics algorithms. Boston/Dordrecht/Lancaster: Kluwer Academic Publishers, 1987.
[8] Roy Featherstone. Rigid body dynamics algorithms. Springer, 2014.
[9] Johan From. An explicit formulation of
singularity-free dynamic equations of mechanical systems in lagrangian form—part one:
Single rigid bodies. Modeling, Identification
and Control, 33(2):45–60, 2012.
[10] Pål J From, Vincent Duindam, Kristin Y
Pettersen, Jan T Gravdahl, and Shankar
Sastry. Singularity-free dynamic equations
of vehicle–manipulator systems. Simulation
Modelling Practice and Theory, 18(6):712–
731, 2010.
[11] Dieter Jungnickel and Tilla Schade. Graphs,
networks and algorithms. Springer, 2008.
[12] Jerrold E. Marsden and Tudor Ratiu. Introduction to Mechanics and Symmetry.
Springer-Verlag New York, 1999.
[13] Richard M Murray, Zexiang Li, S Shankar
Sastry, and S Shankara Sastry. A mathematical introduction to robotic manipulation.
CRC press, 1994.
[14] F.C. Park and J.E. Bobrow. A recursive algorithm for robot dynamics using lie groups.
In IEEE International Conference on Robotics and Automation, volume 2, pages 1535–
1540, Salt Palace Convention Center, Salt Lake City, UT, 1994.
[15] Frank C. Park, James E Bobrow, and Scott R
Ploen. A lie group formulation of robot dynamics. The International Journal of Robotics
Research, 14(6):609–618, 1995.
[16] Franz Reuleaux. The Kinematics of Machinery: Outlines of a Theory of Machines.
Translated and Edited by Alexander BW Kennedy, and with a New Introd. by Eugene S.
Ferguson. Dover Publications, 1963.
[17] Jon M Selig. Geometric fundamentals of robotics. Springer Science & Business Media,
2005.
[18] Stefano Stramigioli. From differentiable manifold to interactive robot control. PhD thesis, TU Delft, Delft University of Technology,
1998.
[19] Stefano Stramigioli.
Modeling and IPC
control of interactive mechanical systems:
a coordinate-free approach. Springer-Verlag
New York, Inc., 2001.