Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...

PP := L1 ← 0
M1 ← 0
N1 ← 1
L2 ← cv M2 ← sv N2 ← 0
P1 ← sv ⋅ rv Q1 ← −cv ⋅ rv R1 ← 0
P2 ← 0
Q2 ← 0
R2 ← 0
a1 ← 0
a2 ← 0
a3 ← 0
a4 ← 0
a6 ← 0
for k∈3.. 5
PP
P
( ) sin( α k)
( )
a1k ← M k−2⋅Nk−1 − Nk−2⋅Mk−1 ⋅ + Lk−2⋅cos α k
b1k ← ( Nk−2⋅Lk−1 − Lk−2⋅Nk−1) ⋅ sin( α k)
+ M k−2⋅cos ( α k)
c1k ← ( Lk−2⋅Mk−1 − M k−2⋅Lk−1) ⋅ sin( α k)
+ Nk−2⋅cos ( α k)
d1k ← ( M k−2⋅Rk−1 − R k−2⋅Mk−1 + Nk−1⋅Qk−2 − Nk−2⋅Qk−1 − ak ⋅ Lk−2) ⋅ sin( α k)
+ ak ⋅ ( M k−2⋅Nk−1 − Nk−2⋅Mk−1) ⋅ Pk−2 ⋅
e1k ← ( Nk−2⋅Pk−1 − Pk−2⋅Nk−1 + Lk−1⋅Rk−2 − Lk−2⋅Rk−1 − ak ⋅ M k−2) ⋅ sin( α k)
+ a k ⋅ ( Nk−2⋅Lk−1 − Lk−2⋅Nk−1) ⋅ Qk−2 ⋅
f1k ← ( Lk−2⋅Qk−1 − Qk−2⋅Lk−1 + M k−1⋅Pk−2 − M k−2⋅Pk−1 − ak ⋅ Nk−2) ⋅ sin( α k)
+
Lk ← a1k M k ← b1k Nk ← c1k Pk ← d1k Qk ← e1k R k ← f1k a k ⋅ ( Lk−2⋅Mk−1 − M k−2⋅Lk−1) ⋅ R k−2 ⋅
substitute , α5 simplify
collect, rv collect, sin( α3) π
2
→
( )
( )
( )
⎡⎣ ⎤⎦ cos α k
⎡⎣ ⎤⎦ cos α k
⎡⎣ ⎤⎦ cos α k
Figure B.2 – MathCAD Program to Find Moments of Unit Vector Expressions
81