4
MANIPULATOR RIGID-BODY DYNAMICS
21
joint i-1
joint i
joint i+1
link i-1
link i
T
i-1
T
i
a
i
X
i
Y
i
Z
i
a
i-1
Z
i-1
X
i-1
Y
i-1
p*
v
i
.
vi
.
i
i
n f
i i
N F
i
i
v
i
.
vi
_ _
i+1 i+1
n f
s
i
Figure 9: Notation used for inverse dynamics, based on standard Denavit-Hartenberg nota-
tion.
Outward recursion, 1
i n.
If axis i
+ 1 is rotational
i
+1
i
+1
=
i
+1
R
i
i
i
+ z
0
q
i
+1
(12)
i
+1
i
+1
=
i
+1
R
i
i
i
+ z
0
¨q
i
+1
+
i
i
× z
0
q
i
+1
(13)
i
+1
v
i
+1
=
i
+1
i
+1
×
i
+1
p
i
+1
+
i
+1
R
i
i
v
i
(14)
i
+1
v
i
+1
=
i
+1
i
+1
×
i
+1
p
i
+1
+
i
+1
i
+1
×
i
+1
i
+1
×
i
+1
p
i
+1
+
i
+1
R
i
i
v
i
(15)
If axis i
+ 1 is translational
i
+1
i
+1
=
i
+1
R
i
i
i
(16)
i
+1
i
+1
=
i
+1
R
i
i
i
(17)
i
+1
v
i
+1
=
i
+1
R
i
z
0
q
i
+1
+
i
v
i
+
i
+1
i
+1
×
i
+1
p
i
+1
(18)
i
+1
v
i
+1
=
i
+1
R
i
z
0
¨q
i
+1
+
i
v
i
+
i
+1
i
+1
×
i
+1
p
i
+1
+ 2
i
+1
i
+1
×
i
+1
R
i
z
0
q
i
+1
+
i
+1
i
+1
×
i
+1
i
+1
×
i
+1
p
i
+1
(19)
i
v
i
=
i
i
× s
i
+
i
i
×
i
i
× s
i
+
i
v
i
(20)
i
F
i
= m
i
i
v
i
(21)
i
N
i
= J
i
i
i
+
i
i
× J
i
i
i
(22)
Inward recursion, n
i 1.
i
f
i
=
i
R
i
+1
i
+1
f
i
+1
+
i
F
i
(23)
i
n
i
=
i
R
i
+1
i
+1
n
i
+1
+
i
+1
R
i
i
p
i
×
ii
+1
f
i
+1
+
i
p
i
+ s
i
×
i
F
i
+
i
N
i
(24)
Q
i
=
i
n
i
T
i
R
i
+1
z
0
if link i
+ 1 is rotational
i
f
i
T
i
R
i
+1
z
0
if link i
+ 1 is translational
(25)
where
Summary :
If axis i + 1 is rotational i +1 i +1 = i +1 R i i i + z 0 q i +1 (12) i +1 i +1 = i +1 R i i i + z 0 ¨q i +1 + i i × z 0 q i +1 (13) i +1 v i +1 = i +1 i +1 × i +1 p i +1 + i +1 R i i v i (14) i +1 v i +1 = i +1 i +1 × i +1 p i +1 + i +1 i +1 × i +1 i +1 × i +1 p i +1 + i +1 R i i v i (15) If axis i + 1 is translational i +1 i +1 = i +1 R i i i (16) i +1 i +1 = i +1 R i i i (17) i +1 v i +1 = i +1 R i z 0 q i +1 + i v i + i +1 i +1 × i +1 p i +1 (18) i +1 v i +1 = i +1 R i z 0 ¨q i +1 + i v i + i +1 i +1 × i +1 p i +1 + 2 i +1 i +1 × i +1 R i z 0 q i +1 + i +1 i +1 × i +1 i +1 × i +1 p i +1 (19) i v i = i i × s i + i i × i i × s i + i v i (20) i F i = m i i v i (21) i N i = J i i i + i i × J i i i (22) Inward recursion, n i 1.
Tags :
link,joint,recursion,translational,dynamics,rotational,axis,nota,outward,standard,used,rigidbody,inerse