Musculoskeletal Dynamics
Grant Schaffner
ATA Engineering, Inc.
Part II
Musculoskeletal Dynamics
Free Body Diagram of Link
i
0
x
0
y
0
z
0
O
1
?
i
O
i
O
Joint
i
Joint
i
+1
g
i
m
Link
i
i
i
,
1
?
r
ci
i
,
r
ci
i
,
1
?
r
i
i
,
1
?
N
1
,
+
?
i
i
N
i
i
,
1
?
f
1
,
+
?
i
i
f
ci
v
i
ω
i
I
Lagrangian Formulation of Equations of Motion
?
D
escr
ibes the behav
ior of a
dynamic system in terms of work and
energy stored in the system
?
C
onstraint forces are automatically elim
inated (an advantage and
a disadvantage), called the “closed form” dynamic equations
?
E
quations are deriv
ed systematically (eas
ier
to use)
i
i
i
i
i
i
i
n
q
Q
n
i
Q
q
L
q
L
dt
d
q
q
L
q
q
coord
d
generalize
to
ing
correspond
force
d
generalize
:
from
derived
then
are
motion
of
Equations
:
Lagrangian
Define
energy
potential
total
energy
kinetic
total
system
dynamic
a
of
s
coordinate
d
generalize
,
,
1
)
,
(
,
,
1
=
=
=
?
?
?
?
?
?
=
=
=
=
m
D
D
m
U
T
U
T
(2-1)
Lagrangian Formulation
?
C
ompute the veloc
i
ty and angular velocity of an indiv
i
dual link i
(think of th
e link as an end effector with coord
sys a
t
th
e link c.m.)
q
J
J
J
ω
q
J
J
J
v
D
D
l
D
D
D
l
D
)
(
)
(
1
)
(
1
)
(
)
(
1
)
(
1
i
A
i
i
A
i
i
A
ci
i
L
i
i
Li
i
L
ci
q
q
q
q
=
+
+
=
=
+
+
=
wh
ere
j
-th column vectors of the 3xn Jacobian
matrices
link
i
,
i.e.,
)
(
i
Lj
J
)
(
i
Aj
J
)
(
i
L
J
)
(
i
A
J
]
[
]
[
)
(
)
(
1
)
(
)
(
)
(
1
)
(
0
0
J
J
J
0
0
J
J
J
h
h
h
h
i
A
i
i
A
i
A
i
Li
i
L
i
L
=
=
Note:
tion of link
i
depends on only joints 1 through
i
,
the column vectors are set to
zero for
j
≥
(2-2) (2-3)
are the
and
for the linear and angular velocities of
and
Since the mo
i
Lagrangian Formulation
?
E
ach column vector is given by:
?
?
?
=
?
?
?
×
=
?
?
?
?
0
b
J
b
r
b
J
1
)
(
1
,
1
1
)
(
j
i
Aj
j
ci
j
j
i
Lj
(revolute jt) (prismatic jt) (revolute jt) (prismatic jt)
=
?
ci
j
,
1
r
Pos
i
tion vector of centroid
of link
i
wrt inboard link
coordinate frame
=
?
1
j
b
3x1 unit vector along joint axis
j
-1
(2-4)
(
∑
=
+
=
n
i
i
i
T i
ci
T ci
i
m
1
2 1
2 1
ω
I
ω
v
v
T
(2-5)
wh
ere
frame
coord
base
wrt
centroid,
the
at
tensor
inertia
link
of
mass
3
3
×
=
=
i
i
i
m I
Note:
wrt the base coord
f
rame
i
I
)
varies with the orientation of the link
Lagrangian
F
ormulation
Note:
tensor defined
relativ
e
to
the coord frame fixed to the link, using
(
(
)
(
1
)
(
)
(
)
(
)
(
2 1
1
)
(
)
(
)
(
)
(
2 1
definite
positive
symmetric
is
n)
(n
tensor
inertia
system
where
H
J
I
J
J
J
H
q
H
q
q
J
I
J
q
q
J
J
q
T
×
=
+
=
=
+
=
∑
∑
=
=
n
i
i
A
i
T
i
A
i
L
T
i
L
i
T
n
i
i
A
i
T
i
A
T
i
L
T
i
L
T
i
m
m
?
?
?
?
?
?
i
I
i
I
T
i
i
i
i
0
0
R
I
R
I
=
(2-6)
(2-7)
(2-8)
, the inertia
can be obtained from
)
)
Lagrangian
F
ormulation
Potential Energy
∑
=
=
n
i
ci
T
i
m
1
,
0
r
g
U
(2-9)
Generalized Forces
ext
T
F
J
τ
Q
+
=
torques
joint
=
τ
moments
and
forces
external
=
ext
T
F
J
Lagrange’s Equations of Motion
(see
Asada & Slotine fo
r d
e
r
ivation)
(
n
i
Q
G
q
q
h
q
H
i
i
k
n
j
j
n
k
ijk
j
n
j
ij
,
,
1
1
1
l
C
C
C
C
=
=
+
+
∑∑
∑
=
=
i
jk
k
ij
ijk
q
H
q
H
h
?
?
?
?
?
=
2 1
(2-11)
(2-10)
Inertia
torque
s
Cent
rifugal/
Coriolis t
r
qs
Gr
a
v
ity
torque
G
e
ner
a
liz
e
d
Force
s
)
(
1
j
Li
n
j
T
j
i
m
G
J
g
∑
=
=
)
1
=
Example: 2 dof planar arm
?
V
e
l
oc
ities of centroids
c
1
and
c
2
0
x
0
y
1
l
2
l
1
1
,
τ
θ
2
2
,
τ
θ
1
c
l
2
c
l
1
1
,
m
I
2
2
,
m
I
q
v
?
??
?
??
?
?
=
0
cos
0
sin
1
1
1
1
1
θ
θ
c
c
c
l
l
q
v
?
??
?
??
?
+
+
+
+
?
+
?
?
=
)
cos(
)
cos(
cos
)
sin(
)
sin(
sin
2
1
2
2
1
2
1
1
2
1
2
2
1
2
1
1
2
θ
θ
θ
θ
θ
θ
θ
θ
θ
θ
c
c
c
c
c
l
l
l
l
l
l
?
T
hese 2x2 matrices are the
?
a
ssociated with the angular velocities
are 1x2 row vectors in this planar case
)
(
i
L
J
)
(
i
A
J
q
q
?
?
?
?
?
]
1
1
[
]
0
1
[
2
1
2
1
1
=
+
=
=
=
θ
θ
ω
θ
ω
Example: 2 dof planar arm
?
S
ubstituting the linear
and angular Jacobians into eqn (2-7) gives
q
H
C
?
?
?
?
?
?
+
+
+
+
+
+
+
+
+
+
=
2
2
2
2
2
2
2
2
2
2
1
2
2
2
2
2
2
2
1
2
2
2
2
1
2
2
2
1
2
1
2
1
1
cos
cos
)
cos
2
(
I
l
m
I
l
m
l
l
m
I
l
m
l
l
m
I
l
l
l
l
m
I
l
m
c
c
c
c
c
c
c
c
θ
θ
θ
0
,
0
,
sin
sin
2
,
sin
,
0
221
212
222
2
2
1
2
211
2
2
1
2
121
112
2
2
1
2
122
111
=
+
=
=
?
=
+
?
=
=
h
h
h
l
l
m
h
l
l
m
h
h
l
l
m
h
h
c
c
c
θ
θ
θ
?
C
entrifugal / Coriolis term coefficients
?
G
ravity ter
m
s
?
S
ubstituting the above into (2-11) gives
]
[
]
[
)
2
(
2
2
)
1
(
2
1
2
)
2
(
1
2
)
1
(
1
1
1
L
L
T
L
L
T
m
m
G
m
m
G
J
J
g
J
J
g
+
=
+
=
2
2
2
1
211
1
12
2
22
1
1
2
1
121
112
2
2
122
2
12
1
11
)
(
τ
θ
θ
θ
τ
θ
θ
θ
θ
θ
=
+
+
+
=
+
+
+
+
+
G
h
H
H
G
h
h
h
H
H
C
C
C
C
C
C
C
C
C
C
C
C