Академический Документы
Профессиональный Документы
Культура Документы
ABSTRACT
Analytical formulas for kinematic and kinetic derivatives needed in multibody system analyses are derived. A broad
spectrum of problems, including implicit numerical integration, dynamic sensitivity analysis, and kinematic workspace
analysis, require evaluation of first derivatives of generalized inertia and force expressions and at least three derivatives of
algebraic constraint functions. In the setting of a formulation based on Cartesian generalized coordinates with Euler
parameters for orientation, basic identities are developed that enable practical and efficient computation of all derivatives
required for a large number of multibody mechanical system analyses. The formulation is verified through application to to a
spatial slider crank mechanism and a 14 body vehicle model. Efficiency of computation using the expressions derived is
compared with results obtained employing finite differences, showing significant computational advantage using the
analytically derived expressions.
1 INTRODUCTION
Three different areas of multibody system analysis are considered. The common requirement for numerical methods used
in these types of analysis is the availability of higher order derivatives of both the differential equations of motion and of the
algebraic constraint equations. The three types of analysis under consideration are as follows:
(1) numerical implicit integration of the differentialalgebraic equations (DAE) of motion for simulation of stiff
mechanical systems;
(2) dynamic sensitivity analysis for design optimization, parameter estimation, and model correlation;
(3) kinematic workspace analysis of mechanisms.
Next, each of these problems is shortly described and required derivatives for numerical solution are identified.
^ ..
(1)
M q q Q
(q, t) 0
(2)
..
where q, q, and q are n 1 vectors representing generalized positions, velocities, and accelerations; M(q) is the n n
^
symmetric generalized inertia, or mass matrix; Q(q, q, t) is the n 1 generalized force vector; is the m 1 vector of
^
i(q, t)
q j
the kinematic constraint Jacobian matrix has full row rank. Equations 1 and 2 thus represent an index3 DAE (Brenan et al.,
1989).
The position constraints of Eq. 2 can be differentiated with respect to time to yield the kinematic velocity and acceleration
constraint equations,
^
..
qq t 0
^
(3)
.
qq qq q 2 tqq tt 0
(4)
derivatives of all terms in Eqs. 1 through 4 with respect to both generalized coordinates and velocities are required. The
^ ..
..
q q , q q .
^
following quantities related to the kinematics of the underlying mechanism must be evaluated:
F l , F l a ,
^T
q
^T
q
^T
F ql a
q
M0 4G 0JGpr ) FF F0 l + 2G n )F 8G JGp
..
..
T
r
T
p
PT
p
. T
(5)
F(r, p, t) + 0
(6)
F P(p) + 0
(7)
where M + diag{m iI3} is a blockdiagonal matrix containing the masses of the n b bodies, J + diag{J i} is the blockdiagonal
inertia matrix of the system, r + {r i} is the vector of body positions, p + {p i} is the vector of Euler parameters, F A is the vector
of applied forces, n A is the vector of applied torques, G is a 34 matrix defined as G(p) + [* e, * e~ ) e 0I] , and F P is the
vector of Euler parameter normalization constraints. Here, e~ is the 33 skewsymmetric matrix defined by
e0
e
e
~
e3 e2
0 e1
e1
0
(8)
As shown in this paper, a relatively small number of basic identities enables computation of all required derivatives of both
kinematic and kinetic terms arising in each of the analyses discussed above. Explicit forms for derivatives of basic quantities
are obtained. Therefore, this approach is expected to be more efficient than using automatic differentiation methods (Bischof et
al., 1996). When evaluating higher order derivatives, automatic differentiation codes differentiate a lower order derivative and
thus cannot take advantage of simplifying identities that may exist, such as Euler parameter normalization constraints.
2 BASIC IDENTITIES
Derivatives of expressions used as basic building blocks in generating required kinematic and kinetic derivatives are
obtained in this section. The identities derived are subsequently applied to obtain higher order derivatives of the constraint
equations and of common force elements. When using Cartesian coordinates with Euler parameters to model a multibody
system, the generalized inertia matrix has a simple blockdiagonal form. Obtaining derivatives of terms involving this matrix
is therefore straightforward.
In the Cartesian coordinate formulation, dependencies on states, velocities, and parameters that appear in the coefficients
of the DAE of motion are well defined. The most complicated terms are those involving the Jacobian of the constraint
equations. Derivatives with respect to Euler parameters are the principal focus, since dependency on position vectors is
straightforward.
A quantity that arises in many terms of interest is representation of a bodyfixed vector relative to the global reference
frame. Let a be a constant bodyfixed vector, represented in the body reference frame, and let p e 0; e T be the vector of
T
Euler parameters that defines the orientation of the body reference frame relative to a global reference frame. Then, the
orientation transformation matrix is (Haug, 1989)
A(p) (2e 20 1)I 2(ee T e 0e~ )
(9)
(10)
The derivative of this quantity with respect to the vector of Euler parameters is given by the following sequence of operations:
~
(A(p)a ) 4e a 2a
e
0
e 0
(11)
~
~
(A(p)a ) 2 e Ta I ea T e a
2 a e T ea T (e 0I e~ )a
0
e
(12)
~
(A(p)a) + (A(p)a) ; (A(p)a) + 2 ap T ) (e I ) e~ )a; ea T * (e I ) e~ )a
0
0
p
e 0
e
(13)
B(p, a) 5 (e 0I ) e~ )a; ea T * (e 0I ) e~ )a
(14)
B(p, a) 5 2 ap T ) B(p, a)
(15)
(16)
Note that the 33 orientation transformation matrix A(p) involves quadratic terms in p, so it is expected that linear
^
expressions in p will occur after differentiation with respect to p in Eq. 13. Indeed, the matrices B and B are linear in the Euler
parameters. Moreover, these matrices have a commutativity property involving two 4dimensional vectors, p i and p j, namely,
~
(17)
and
B(p i, a)p j + 2 ap Tip j ) B(p i, a)p j + 2 ap Tjp i ) B(p j, a)p i + B(p j, a)p i
(18)
(19)
Calculation of the derivative of B T(p, a) with respect to p is also required. Note, from Eq. 14, that
^T
B (p, a) +
a T(e 0I * e~ )
~
(20)
ae T ) a(e 0I * e~ )
^T
B
(p, a) +
e 0
a T
(21)
a
^T
B
(p, a)g +
e
a Tg~
~
ag T ) ag~
(22)
^T
B
(p, a)g +
p
a Tg
a Tg~
ag ag T ) ag~
(23)
Using the result of Eq. 23, together with the definition of Eq. 15,
+ D(a, g)
^T
B T(p, a)g + 2 pa Tg ) B
(p, a)g
p
p
p
(24)
D(a, g) 8 2
2a Tg
a Tg~
ag a TgI ) ag T ) ag~
+2
2a Tg
~
a Tg~
~
ag ag~ ) g~ a ) 2a TgI
(25)
~
T
T
The final form of Eq. 25 shows that D is symmetric, since a Tg~ + g~ a + * g~ a + ag.
frames relative to the origin of the global reference frame and p i and p j are 41 vectors of Euler parameters that define body
reference frame orientations. Let s i be a vector fixed in body i, so s i + A(p i)s i is its representation in the global frame. Define
s j and s j in a similar fashion. Finally, let d ij + r j ) A js j * r i * A is i define the vector between points P i and P j on bodies i and
(26)
(2) orthogonality of a bodyfixed vector a i on body i and a vector d ij between bodies i and j (DOT2 constraint),
F d 2 + a Tid ij + d Tija i + 0
(27)
(3) fixed distance between points P i on body i and P j on body j (Distance constraint),
ss d Tijd ij C 2 d Tjid ji C 2 0
(28)
(29)
Most kinematic constraints that are encountered in applications can be described in terms of one or more of the above
conditions.
A ball and socket joint (or spherical joint) is defined by the condition that the centers of the ball and of the socket coincide
and is thus equivalent to a basic spherical constraint. The revolute joint of Fig. 2 allows one rotational degree of freedom about
a common axis fixed in bodies i and j and no relative translation along this axis. The five constraint equations that describe this
joint can be written in terms of basic constraints as
s(P i, P j) 0
30
d 1(f i, h j) 0
d 1(g i, h j) 0
The universal joint of Fig. 3 permits relative rotations about two orthogonal axes fixed in the bodies connected. It is described
by the conditions that points P i on body i and P j on body j coincide and that the vectors h i and h j are perpendicular. This can be
written as the following four constraint equations:
s(P i, P j) 0
31
(h i, h j) 0
d1
The translational joint of Fig. 4 has one relative translational degree of freedom along a common axis fixed in the bodies
connected. It is defined by five constraint equations. In terms of basic constraint these are
d 1(f i, h j) 0
d 1(g i, h j) 0
32
d 2(f i, d ij) 0
d 2(g i, d ij) 0
d 1(f i, f j) 0
In a similar way other commonly used physical joints, e.g., cylindrical, screw, or composite joints, can be described in
terms of the four previously defined basic constraints.
In addition to joint kinematic constraints, the Euler parameter normalization constraint,
Pi p Tip i 1 0
(33)
(34)
F s + B(p , s )
j j
p j
With the above relations, the derivative of F s with respect to q ij, with the superscripts i and j suppressed for notational
convenience, becomes
F sq + * I, * B(p i, s i), I, B(p j, s j)
(35)
(36)
(37)
(38)
T
T
T
T
With b T + b 1 , b2 , b3 , b 4 R 14, the product F sqa qb is
(39)
Since this quantity does not depend on q, its derivative with respect to q is zero; i.e,
F a b
s
q
+0
(40)
(41)
F q q
.
s .
q q
(42)
Let l R 3. Then,
B(pl, s )l
F l
l
(p
,
s
)l
B
T
s
q
(43)
00 D(s0 , l) 00 00
F l
0 0 0 0
0 0 0 D(s , l)
T
s
q
(44)
(45)
(46)
F Pia 0, 2a 2 T
q
i
(47)
0 0
F PT
i l q l 0 2I
4
i
(48)
Derivatives of the right side of the Euler parameter acceleration constraint are computed as
10
Pi q. i
q
Pi q. i
q
qi
qi
0
(49)
qi
and
qi
qi
0, 4p i
.T
50
qi
For body i, the derivative of G T(p i)J iG(p i)p i with respect to p i must be evaluated. This quantity can be written as
..
G TiJ iG ip i
..
e i0
e Ti
J i[ e i, e~ i e i0I] e..
e i e i0I
i
~
(51)
(52)
G(pi)p j G(pj)
p
(53)
For any 31 vector x, the derivative of the product G T(p i)x with respect to p i is obtained as
(G T(p i)x) p x
(54)
0 xT
x x x~
(55)
..
Using Eqs. 53 and 54, the derivative G T(p i)J iG(p i)p i p is obtained as
i
(56)
time derivatives. For the generalized force vector Q(q, q), the quantities Q q and Q q are calculated in this section.
.
11
Consider the term G T(p i)J iG(p i)p i in Eq. 5. Its derivative with respect to p i is
G T(p. i)J iG(p. i)p i + G T(p. i)J iG(p. i)
p
(57)
(58)
(59)
f + k(l * l 0) ) cl ) F
(60)
(61)
d Tij
dr j ) B(pj, s j)dpj * dr i * B(p i, s i)dpi
l
(62)
where Eq. 16 has been used. Substituting this expression into Eq. 59 and identifying the coefficients of virtual displacements
and variations in Euler parameters, the TSDA generalized forces acting on bodies i and j are
Qi +
d ij
f
T
l B (p i, s i)d ij
Qj + *
d ij
f
T
l B (p j, s j)d ij
(63)
12
The focus here is on derivatives of Q i. Derivatives of Q j are obtained in a similar fashion. The generalized force Q i acting
QF
on body i is rewritten as Q i Q N , where Q F (fl)d ij is the linear TSDA generalized force and Q N (fl)B T(p i, s i )d ij is the
angular TSDA generalized force.
fl fl d
Q Fq d ij
(64)
ij q
Q Nq B T(p i, s i)d ij
fl fl B (p , s )d
T
(65)
ij q
Since
66
fl0 ; D(s , d ) ; 0 ; 0
i
(67)
ij
(68)
where
ll q dTijdij
,
q
.
d ijd Tij
d ijv T f
f
N1 k c l
c
I
l
l l2
l
l2
N2 c
(69)
d ijd Tij
(70)
l2
the derivatives of Q F (fl)d ij and Q N (fl)B T(p i, s i )d ij with respect to q can be expressed in compact form as
Q Fq N 1, N 1B(p i, s i ) N 2B(p i, s i ),N 1, N 1B(p j, s j ) N 2B(p j, s j )
.
13
(71)
f
.
Q Nq B T(p i, s i)N 1, B T(p i, s i)N 1B(p i, si ) N 2B(p i, si ) Ds i, d ij,
l
(72)
fl
Q Fq d ij
.
(73)
.
Q Nq B T(p i, s i)d ij
.
fl B (p , s )Q
T
F.
q
(74)
fl 1l f
c lq
l
(75)
lq
.
(76)
d Tij
I, B(p i, s i), I, B(pj, s j)
l
(77)
d ijd Tij
l2
Q Nq cB(p i, s i)
.
d ijd Tij
l2
(78)
(79)
7 NUMERICAL EXPERIMENTS
The kinematic and kinetic derivatives obtained in analytical form in this paper are verified using two examples, with
numerical efficiency compared with finite differences. The two mechanisms analyzed are in assembled configurations and
kinematically admissible values for the generalized velocities were used in computations.
14
generalized coordinates, constrained by 17 joint constraints and 3 Euler parameter normalization constraints. The following
.
kinematic derivatives are considered: F q, F Tql q, F qq q , g q, and g q, where F is the array of constraint expressions, l is the
.
.
.
array of Lagrange multipliers, and g F qq qq is the right side of the acceleration constraint equation.
Table 1 presents maximum absolute differences between analytical and numerical values of the kinematic derivatives, for
different values of the perturbation d used in computing finite differences. Analytical Jacobians of generalized force with
respect to generalized coordinates and velocities are also compared to numerical Jacobians obtained with finite differences.
Results are presented in Table 2. Finite difference numerical derivatives are obtained with an accuracy of the order of the
perturbation d. As shown in Tables 1 and 2, they converge to the analytical values as d decreases.
As numerical results show, the perturbation d has a strong influence on computational accuracy using finite differences.
Moreover, selection of d to obtain derivatives within a prescribed accuracy is function of the type of derivative to be computed
and is problem dependent.
Availability of analytical derivatives not only gives more accurate results, but also leads to increased computational
efficiency. Table 3 presents the average CPU seconds required to compute the complete three groups of derivatives (kinematic
derivatives, generalized force derivatives, and generalized mass matrix derivatives) for the slider crank mechanism, using both
analytical and finite difference methods. The numerical experiments were performed on a HP J210 computer. Speedups of
more than 10 were obtained for all derivative computations when using exact analytical formulas.
15
8 CONCLUSIONS
Results presented in this paper enable accurate and efficient computation of higher order derivatives required in multibody
analysis, including implicit numerical integration of the differentialalgebraic equations (DAE) of motion, dynamic
sensitivity analysis of mechanical systems, design optimization, and parameter estimation. A formulation to generate three
derivatives of the terms involved in the DAE of motion required in each of these analyses is presented and verified. This
formulation is extendable to compute higher order derivatives that appear in continuation methods for mapping workspace
boundaries. Derivative computations are developed in the Cartesian coordinate formulation with Euler parameters as
orientation parameters. Similar methods can be applied to generate kinematic and kinetic derivatives when using joint (or
relative) coordinates.
ACKNOWLEDGMENT
This research was supported by the US Army TankAutomotive Command (TACOM), through the Automotive Research
Center (Department of Defense contract number DAAE0794R094).
REFERENCES
Adkins, F.A., 1996, Numerical Continuation and Bifurcation Methods for Mechanism Workspace and Controllability Analysis,
Ph.D. dissertation, The University of Iowa
Bestle, D. and Eberhard, P., 1992, Analyzing and Optimizing Multibody Systems, Mechanics of Structures and Machines, Vol
20(1), pp. 6792
Bestle, D. and Seybold, J., 1992, Sensitivity Analysis of Constrained Multibody Systems, Archive of Applied Mechanics, Vol. 62,
pp. 181190
Bischof, C., Roh, L., and Mauer, A., 1996, ADIC: A Tool for the Automatic Differentiation of C Program, Technical Report,
Mathematics and Computer Science Division, Argonne National Laboratory
Brenan, K.B., Campbell S.L., and Petzold L.R., 1989, Numerical Solution of InitialValue Problems in DifferentialAlgebraic
Equations, North Holland Publishing Co., New York
Chang, C.O. and Nikravesh, P.E., 1985, Optimal Design of Mechanical Systems With Constraint Violation Stabilization Method,
Journal of Mechanisms, Transmissions, and Automation in Design, Vol. 107, pp. 493498
Freudenstein, F. and Primerose, E.J., 1984, On the Analysis and Synthesis of the Workspace of a threeLink, TurningPair
Connected Robot Arm, Journal of Mechanisms, Transmissions, and Automation in Design, Vol. 106, pp. 365370
16
Hairer E. and Wanner G., 1996, Solving Ordinary Differential Equations II: Stiff and DifferentialAlgebraic Problems, Springer
Verlag, Berlin
Haug, E.J., 1987, Design Sensitivity Analysis of Dynamic Systems, ComputerAided Design: Structural and Mechanical Systems,
C.A. MotaSoares, Ed., SpringerVerlag, Berlin
Haug, E.J., 1989, ComputerAided Kinematics and Dynamics of Mechanical Systems, Volume I: Basic Methods, Allyn and Bacon,
Needham Heights, Massachusetts
Haug, E.J. and Ehle, P.E., 1982, SecondOrder Design Sensitivity Analysis of Mechanical System Dynamics, International
Journal for Numerical Methods in Engineering, Vol. 18, pp. 16991717
Haug, E.J., Luh, C.M., Adkins, F.A., and Wang, J.Y., 1996, Numerical Algorithms for Mapping Boundaries of Manipulator
Workspaces, Journal of Mechanical Design, Vol. 118, pp. 228234
Haug, E.J., Negrut, D, and Iancu, M., 1997, An Implicit Numerical Integration Algorithm for DifferentialAlgebraic Equations of
Multibody Dynamics, Mechanics of Structures and Machines, to appear
Haug, E.J., Iancu, M, and Negrut, D., 1997, Implicit Integration of the Equations of Multibody Dynamics in Descriptor Form,
Submitted to the 1997 ASME Design Automation Conference
Jo, D.Y. and Haug, E.J., 1989, Workspace Analysis of Multibody Mechanical Systems Using Continuation Methods, Journal of
Mechanisms, Transmissions, and Automation in Design, Vol. 3, pp. 581589
Krishnaswami, P., Wehage, R.A. and Haug, E.J., 1983, Design Sensitivity Analysis of Constrained Dynamic Systems by Direct
Differentiation, Technical Report No. 835, Center for ComputerAided Design, The University of Iowa, Iowa City, Iowa
Litvin, F.L., 1980, Application of Theorem of Implicit Function System Existence for Analysis and Synthesis of Linkages,
Mechanism and Machine Theory, Vol. 15, pp. 115125
Nikravesh, P.E., 1988, ComputerAided Analysis of Mechanical Systems, PrenticeHall, Englewood Cliffs, New Jersey
Potra, F.A., 1994, Numerical Methods for DifferentialAlgebraic Equations with Application to RealTime Simulation of
Mechanical Systems, ZAMM, Vol. 74(3), pp. 177187
Serban, R., Negrut, D., and Haug, E.J., 1998, High Mobility Multipurpose Wheeled Vehicle (HMMWV) Multibody Models,
Technical Report No. R211, Center for ComputerAided Design, The University of Iowa, Iowa City, Iowa
Tsai, Y.C. and Soni, A.H., 1981, Accesible Region and Synthesis of Robot Arms, Journal of Mechanical Design, Vol. 103, pp.
803811
Wang, J.Y. and Wu, J.K., 1993, Dextrous Workspace of Manipulators, Part II: Computational MEthods, Mechanics of Structures
and Machines, Vol 21(4), pp. 471506
17
Yang, F.C. and Lee, T.W., 1983, On the Workspace of Mechanical Manipulators, Journal of Mechanisms, Transmissions, and
Automation in Design, Vol. 105, pp. 6269
(80)
(81)
Partitioning the 141 vectors a and b as in Eq. 37, the derivatives F dq1a q and F dq1aqb
F dq1a
are
q
d1
q a qb
q
(82)
+ [0 ,a4 B T(p j, sj)B(b 2, si) ) b 4 B T(a4, sj)B(p i, si) ) b 4 B T(p j, sj)B(a2, si) ,
(83)
2T
2T
Derivatives of the right side of the acceleration constraint equation corresponding to the DOT1 constraint with respect to q
.
and q are
F
F
+ [0 ,p j 2B T(p j, sj)B(p i, si) ) B T(p j, sj)B(p i, si) , 0 , p i 2B T(p i, si)B(p j, sj) ) B T(p i, si)B(p j, sj)
(84)
+ 0 , 2sTjB(p i, si) ) 2p j B T(p j, sj)B(p i, si) , 0 , 2sTiB(p j, sj) ) 2p i B T(p i, si)B(p j, sj)
(85)
.
d1 .
q q qq
q
.
d1 .
q q qq .
q
.T
.T
.T
d1
q
.T
0
00 D(s0, s ) 00 B (p , s )B(p
, s )
F l ++ l0
0
0
0
0
0
B
D(s
(p
,
s
)B(p
,
s
)
,
s
)
(86)
18
(87)
Assuming the orientation matrix A i to be orthonormal, the DOT2 constraint can be rewritten as
F d 2 + r j ) s j * r i a i * s Tia i + 0
T
(88)
The derivative of the constraint of Eq. 88 with respect to q is obtained, using Eq. 16, as
F dq2 + * a Ti, (d ij ) s i) TB(p i, a i), a Ti, a TiB(p j, s j)
(89)
Fdq2a
(90)
d2
q a qb
q
(91)
Derivatives of the right side of the acceleration constraint corresponding to the DOT2 constraint with respect to q and q are
.
d2 .
q q qq
q
.
d2 .
q q qq .
q
.T
.T
.T
.T
(92)
.T
.T
.T
.T
.T
(93)
.T
The derivative of the product F dq2 l with respect to q is obtained, using Eqs. 16 and 24, as
T
* B(p , a )
0
0
* B (p0 , a ) D(a
, d ) s ) B (p , a ) B (p , a )B(p , s )
F l + l 0
B(p , a )
0
0
0
D(s , a )
(p
,
s
)B(p
,
a
)
0
B
d2
q
ij
19
(94)
(95)
(96)
(97)
(98)
F
ss
q a qb
q
(99)
(100)
Derivatives of the right side of the acceleration constraint corresponding to the distance constraint with respect to q and q are
F
.
ss .
q q qq
q
.T
(101)
4ri rj p TiB T(p i, si ) p TjB T(p j, sj )B(p i, si ) 2p i B T(p i, si ) p j B T(p j, sj )B(p i, si ) ,
.T
.T
.T
.T
.T
4ri rj p TiB T(p i, si ) p TjB T(p j, sj )B(p j, sj ) 2p i B T(p i, si ) p j B T(p j, sj )B(p j, sj )
.T
F
.
ss .
q q qq .
q
.T
.T
.T
.T
(102)
.T
.T
.T
The derivative of the product F ssq l with respect to q is obtained, using Eqs. 16 and 24, as
T
20
* 2I
* 2B(p , s )
, s )
* 2B(p , s )
2I
2B(p , s )
(p
,
s
)
(p
,
s
)B(p
,
s
)
*
2B
*
2B
(p
,
s
)
,
d
)
)
2B
(p
,
s
)B(p
,
s
)
2B
2D(s
ss
q
i i
T
ji
ij
j j
T
(103)
0
0
Q i + 2nG Th + 2n G Th
i i
i i
(104)
0
0
Q j + * 2nG Th j + * 2n G Th j
j
j
(105)
where h i and h j are unit vectors along the common axis of rotation, represented in the local frames of bodies i and j,
respectively. The magnitude of the torque n exerted by the RSDA is
.
n + k(q ) 2n revp) ) cq ) N
(106)
.T .T .T .T
are
Q Ni + 2G Tih in q ) 2n[0, h i *, 0, 0]
q
(107)
Q Ni + 2G Tih in q
q
(108)
where the matrix h i * is defined as in Eq. 55 and the components of n q and n q are
.
n+0
ri
(109)
n + k(fTf )fTB(p , g ) * (g Tf )fTB(p , f ) ) cp. TB T(p , f ) @ (fTf )B(p , g ) * (g Tf )B(p , f )) fT(fTf )B(p. , g ) * fT(g Tf )B(p. , f )
i i
i i
j j
i i
i i
i j j
i j j
i j
i j
j i j
j
i j
j
i i
i i
p i
(110)
n+0
rj
(111)
n + k(fTf )g TB(p , f ) * (g Tf )fTB(p , f ) ) cp. T(fTf )B(p , g ) * (g Tf )B(p , f )B(p , f )) g T(fTf )B(p. , g ) * fT(g Tf )B(p. , f )
j j
j j
i i
i i
j j
i j i
i j i
i j
i j
i i j
i
i j
i
i i
i i
p j
(112)
. n + 0
ri
(113)
21
(114)
. n 0
rj
(115)
(116)
In Eqs. 109 and 113, {f i, g i, h i} and f j, g j, h j are unit vectors along axes of the local frames on bodies i and j, respectively.
.
Derivatives of the angular component of Q j, Q Nj 2nG T(p j)h j, with respect to q and q are computed in a similar way.
22
i
hj
Pj
gi
j
fi
Pi
gj
hi
fj
23
hi
fj
Pi P
j
fi
hj
gi
gj
fj
Pj
j
gi
fi
hj
dij
gj
hi
Pi
24
25
26