Spatial Operator Algebra for Multibody System Dynamics 1

Home , Operator algebra

The Journal of the Astronautical Sciences, vol. 40, pp. 27-50, Jan-March 1992

Spatial Op erator Algebra for Multib o dy

System Dynamics

2 2 3

G. Ro driguez , A. Jain and K. Kreutz{Delgado

Abstract

This pap er describ es a new spatial operator algebra for the dynamics of general{top ology

rigid multib o dy systems. Spatial op erators allow a concise and systematic formulation of the

dynamical equations of motion of multib o dy systems and the development of ecient computational

algorithms. Equations of motion are develop ed for progressively more complex systems: serial

chains, top ological trees, and closed{lo op systems. New op erator factorizations and expressions

for the mass matrix and its inverse are derived and used to obtain ecient, spatially recursive

computational algorithms. The algorithms can be easily recon gured in resp onse to changes in

the constraints and the top ology of constituent b o dies. Thus, they are particularly suited for

time{varying multib o dy systems. References are provided for extensions to exible multib o dy

systems. Spatially recursive algorithms, based on the sequential ltering and smo othing metho ds

encountered in optimal estimation theory, provide the computational infrastructure to mechanize

the spatial op erators.

Intro duction

There are two ma jor current challenges in multib o dy system dynamics: achieving com-

putational eciency for increasingly complex systems; and retaining algorithmic eciency while

accounting for p ossible event-dep endentchanges to the constraints and top ology of the constituent

b o dies. These challenges o ccur for example in highly complex and interactive spacecraft and in

space rob otic systems which require that the dynamics algorithms be quickly recon gured in re-

sp onse to con guration changes without sacri cing computational eciency.

In recent years, there has b een signi cant progress in the formulation of the equations of

motion and on the development of ecient dynamics algorithms for multib o dy systems. A large

1 rd

Presented at the 3 Annual Conference on Aerospace Computational Control, August 28{30, 1989, Oxnard, CA

Jet Propulsion Lab oratory/California Institute of Technology, 4800 Oak Grove Drive, Pasadena, CA 91109

AMES Department, University of California, San Diego, La Jolla, CA 92093 1

part of this work, [1 ]{[6 ], has fo cused on the development of recursive computational algorithms

for the dynamics of serial rigid multib o dy systems. References [5 , 7] describ e recursive forward

dynamics algorithms for tree top ology rigid multib o dy systems. References [8 , 9] contain recursive

algorithms sp eci c to closed top ology rigid multib o dy systems consisting of multiple rob ot manipu-

lators grasping a rigid ob ject. References [10 , 11 ] describ e recursive algorithms for general top ology

rigid multib o dy systems.

The spatial operator algebra [8, 9] arose from a recognition of the close parallels b etween

the structure of the equations of motion for serial chain dynamics and those encountered in the area

of optimal estimation theory [12 ]. These parallels naturally led to the intro duction of spatial op er-

ators to obtain a concise formulation of the equations of motion of multib o dy systems. The spatial

op erators have b een used to obtain new op erator factorizations and expressions for the mass matrix

and its inverse. These op erator expressions make it p ossible to recognize high-level mathematical

patterns asso ciated with the mass matrix which the detailed algorithms do not reveal. Therefore,

the number of symb ols that the analyst has to see is reduced signi cantly. In addition, the spa-

tial op erators are mechanized byvery ecient spatially recursive computational algorithms whose

complexity dep ends only linearly on the number of degrees of freedom dofs. These algorithms

closely resemble the algorithms used for recursive ltering and smo othing in the Kalman ltering

and estimation problems. There is the reassuring presence of such familiar concepts as Riccati

equations and Kalman gains. This makes it easier to mechanize the dynamics algorithms and to

monitor their numerical stability and robustness. Much of the exp erience gained over manyyears

of research in estimation theory can now b e used to solvemultib o dy system dynamics problems.

In this pap er, the spatial op erator algebra is advanced as a new systematic pro cedure for

concisely formulating the equations of motion and deriving ecient spatially recursive dynamics

algorithms for general top ology rigid multib o dy systems. Spatial op erator formulations of the

dynamics of serial chain, tree and closed chain top ology multib o dy systems are develop ed in a

progressive sequence. It is seen that the spatial op erator formulation of the dynamics for serial

and tree top ology systems are identical in form. Consequently, such results as the op erator factor-

ization and inversion of the mass matrix for serial chains are directly applicable to tree top ology

systems. Moreover, the recursive computational algorithms for serial chains extend with only minor

mo di cations to tree top ology systems.

Closed top ology systems are mo deled as consisting of primary and secondary tree-top ology

subsystems along with additional kinematical closure constraints within and between them. The

primary system represents the ma jor part of the system whose top ology and constituent b o dies do

not change with time. The secondary system is much smaller in most applications, and represents 2

the part of the system whose top ology and constituent b o dies do change with time.

A recursive computational algorithm is then develop ed for closed{chain systems. This algo-

rithm contains separate steps for the dynamics of the primary and secondary systems using recursive

algorithms for tree{top ology systems, plus additional steps to handle the closure constraints. A

signi cant feature of these algorithms is that only relatively small and lo calized changes are needed

for recon guration in resp onse to changes in either the top ology, the constraints or the constituent

b o dies of the system. The overall computational complexity of the algorithm stays linear in the

number of degrees of freedom in the system. In addition, there is a linear dep endency in the

absence of kinematical singularities on the numb er of closed lo ops in the system.

Extensions of the spatial op erator algebra and the computational algorithms to exible

multib o dy system dynamics are discussed in reference [13 ].

6-Dimensional Spatial Notation

Co ordinate{free 6-dimensional spatial notation used throughout this pap er. Given the linear

and angular velo cities v and ! , the linear force F , and moment N in R at a p oint on a body, the

inertial spatial velocity V , inertial spatial acceleration and the spatial force f in R are:

0 1 0 1

! N

4 4 4

@ A @ A

V = ; = V; f =

v F

Here \ _ " implies the time derivative in an inertial frame. The rigid body transformation operator

x; y 2 R for two p oints x and y is:

0 1

I l

4 x;y

@ A

l =x; y =

x;y

0 I

where l 2 R is the vector joining the two p oints x and y . l is the cross{pro duct matrix

x;y x;y

asso ciated with l which acts on a vector to pro duce the cross{pro duct of l with the vector.

x:y x;y

x; y and x; y transform spatial forces and spatial velo cities resp ectively between the two

b o dy{ xed p oints x and y on a rigid b o dy. The spatial inertia M 2 R of a rigid b o dy at at a

p oint O on the b o dy is

0 1

J O mp~

@ A

M O = =

mp~ mI

where p 2 R is the vector from O to the center of mass of the body, m is the mass of the body,

and J O 2 R is the inertia tensors for the b o dy ab out O . See [14 ] for more discussion on the

prop erties and use of the spatial notation. 3

Serial Rigid Multib o dy Systems

This is the simplest case of a rigid multib o dy system. The system consists of n rigid b o dies

connected together bymultiple dof hinges. The b o dies are numb ered 1 through n from tip to base.

The term outboard inboard b o dy refers to a b o dy on the path towards the tip base.

The generalized co ordinates for the serial chain are the collection of the hinge con guration

parameters. It is assumed that the k hinge p ossesses r k con guration degrees of freedom which

r k

are parameterized by the vector of con guration variables k 2 R and that its r k r k

v p

r k

motion dofs are parameterized by the generalized velo city vector k 2 R . The kinematical

equations which relate k to k dep end on the sp eci c nature of the k hinge. It is assumed

for notational convenience that all the hinge constraints are homogeneous i.e., catastatic. H k

6r k th

is de ned such that H k 2 R is the hinge map matrix for the k hinge. Its columns span

the space of p ermissible relative spatial velo cities across the hinge. The number of overall motion

dofs for the serial chain are given by N = r k for the chain. The state of the multib o dy

k =1

system is de ned by the collection of [ :; :] pairs for all the hinges, and is assumed known.

Since each body is rigid, it suces to develop the equations of motion for a body ab out

a single reference p oint on the body, which is chosen here as the inb oard hinge lo cation O for

th 6 6

the k body see Fig. . With V k 2 R denoting the inertial spatial velo city, k 2 R

6 r k

the inertial spatial acceleration, f k 2 R the spatial interaction force and T k 2 R the

generalized hinge force ab out O for the k body, the following Newton{Euler recursive equations

describ e the equations of motion for the serial chain:

V n +1= 0; n +1= 0

for k = n 1

V k = k +1;kV k +1+H k k

k = k +1;k k +1+H k k+ak

end lo op

0.1

f 0 = 0

for k = 1 n

fk = k +1;kfk 1 + M k k +bk

Tk = Hkfk

end lo op 4

THIS IS THE new links FIGURE

Figure 1: Illustration of b o dies and joints in a serial rigid multib o dy system 5

where the velo city dep endent Coriolis and centrifugal acceleration term ak 2 R is,

ak = k +1;kV k +1+H k k 0.2

The velo city dep endent gyroscopic force term bk 2 R is

bk = M k V k pk M k V k

k; k 1 2 R denotes the transformation op erator from O to O . For more details on the

k 1 k

derivation of these equations of motion, see [9 , 14].

The simplifying assumption is made that the tip force f 0 is zero. Base mobility can

easily b e handled by attaching a full motion 6{dof hinge b etween the physical base and the inertial

frame. For the inverse dynamics problem, the hinge accelerations are known, and equation 0.1

represents an O N computation involving a base{to{tip recursion to compute the velo cities and

accelerations, followed by a tip{to{base recursion to compute the hinge forces.

The equations of motion equation 0.1 can be expressed more concisely using spatial op-

6 n

erators. In this notation, the V k 's, k 's etc are viewed as comp onents of vectors V 2 R ,

6 n

2 R etc. Then, equation 0.1 can b e written in the following compact form:

V = E V + H

= E + H + a

f = E f + M + b 0.3

T = Hf

where,

0 1

0 0 0 0 0

B C

2; 1 0 ::: 0 0

B C

6 n 6n

B C

E = 2 R 0.4

0 3; 2 ::: 0 0

B C

. . . .

B C

. . . . .

. . . .

B C

@ A

0 0 ::: n; n 1 0

0 1 0 1

M 1 0 ::: 0 H1 0 ::: 0

B C B C

0 M2 ::: 0 0 H2 ::: 0

4 4

B C B C

6n6n N6n

2 R ; H = 2 R M =

B C B C

......

. .

B C B C

......

. .

......

B C B C

0.5

@ A @ A

0 0 ::: Mn 0 0 ::: Hn 6

However, since E is nilp otent E = 0,

0 1

I 0 ::: 0

B C

2; 1 I ::: 0

B C

1 2 6n6n

=IE = I +E +E + + E = 2 R

B C

. . .

B C

. . . .

. . .

B C

0.6

@ A

n; 1 n; 2 ::: I

where,

i; j = i; i 1 j +1;jf or i>j

Thus, equation 0.3 can b e reexpressed as,

V = H

= H + a 0.7

f = M + b=M H + M a + b

T = Hf = HM H + HM a + b

4 4

= M + C; where M = HM H ; and C = HM a + b

NN N

M2R is the mass matrix for the serial chain and C 2 R contains the velo city dep endent

Coriolis, centrifugal and gyroscopic hinge forces. In the terminology of Kane's metho d [15 ], are

the generalized speeds and the elements of H are the partial spatial velocities. The op erator

expression M = HM H is denoted the Newton{Euler Operator Factorization of the mass

matrix.

In terms of the rigid b o dy transformation op erator from the tip to the rst hinge by 1; 0,

the tip spatial velo city, V 0, is given by V 0 = 1; 0V 1. Thus,

0 1

1; 0

B C

6n 6

V 0 = B H ; where B = 2 R

B C

@ A

Thus, the Jacobian matrix J 2 R which maps to the tip velo city V 0 is

J = B H ; and wehave V0 = J 0.8

E , , H , and M are the rst spatial operators encountered. Recursive dynamical algorithms

can b e derived easily by using the state transition prop erties [8] of the elements of spatial op erators 7

suchas E , etc.. For instance, given a vector y , the evaluation of the matrix{vector pro duct y does

not require an O n matrix-vector pro duct computation, and not even the explicit computation

of the elements of . Rather, this pro duct can be evaluated using an O n recursive algorithm

involving only the elements of E and y . This is precisely the corresp ondence b etween the concise

op erator based high{level description of the equations of motion in equation 0.7 and the recursive

algorithmic description in equation 0.1.

Spatially recursive O N forward dynamics algorithms for serial chains have b een develop ed

in [3 ] based on the recognition of the close parallels b etween the structure of the multib o dy dynamics

equations and the equations encountered in Kalman Filtering theory [16 ]. These parallels are used

to obtain a new square Innovations op erator factorization of the mass matrix M and an op erator

expression for its inverse, and form the basis for the recursive algorithms.

Tree Top ology Systems

In this section, the dynamics of tree{top ology rigid multib o dy systems are discussed. A tree

top ology system is viewed here as a set of comp onent serial chains referred to as branches, coupled

together via hinges at their inb oard terminal b o dies. The total number of branches is denoted `.

The index for the branches ranges from 1 `, and consistent with the b o dy numb ering scheme in

the previous section, the inb oard branches are assigned indices larger than those for the outb oard

ones. The inboard function {k is de ned as the index of the direct predecessor branch, i.e., the

th th

inb oard branch to which the k branch is directly connected. The j branch is referred to as a

th th

predecessor of the k branch if it b elongs on the unique path from the k branch to the base,

p p

i.e., if { k = j for some integer p > 0, where { : = { { {: denotes a p{times function

comp osition. Figure illustrates the decomp osition of a tree system into branches as well as a

sample branchnumb ering scheme for the system.

The notation develop ed earlier for serial chains is used to describ e the branches in the tree

system. A subscript serves to identify the sp eci c branch in the system. Thus n and N denote

j j

, and M , E , the numb er of b o dies and the numb er of motion dofs resp ectively, while V 2 R

j j j

th 6n 6n

j j

denote the spatial vector, spatial inertias etc. for the j branch. A b o dy/hinge etc. in R

is identi ed by the index of the branchitison, plus its lo cation within the branch. For instance,

th th

the k body is the k body on the j branch, and V k or V k denotes the spatial velo city

j j j

th th

body of the j branch at its inb oard hinge lo cation O k . The overall stacked of the the k

spatial velo city, acceleration etc. vectors for the tree are once again denoted V , , f etc. with

V =[V V ] 2 R etc. The total numb er of b o dies n, and the total numb er of motion dofs

` 8

THIS IS THE TREE DECOMPOSITION FIGURE

Figure 2: Illustration of decomp osition into branches for a tree system 9

N for the tree multib o dy system are now given by

` `

X X

4 4

n = n and N = N 0.9

j j

j =1 j =1

th th

Note that when the j branch is the direct predecessor of the k branch, i.e., j = {k , the hinge

th th

connecting them is is regarded as the the n hinge the last hinge on the k branch and describ es

th th

the attachment to body 1 on the j branch. The transformation op erator from the n hinge

th 6n 6n

to the 1 hinge is denoted 1 ;n . The spatial op erator E 2 R for the tree system is

de ned in terms of its blo ck matrix elements b elow. For j; k 2 1 `, the j; k blo ck element

6n 6n

E j; k 2 R of E is de ned as follows:

for j = k E

1 0

0 0 1 ;n >

C B

0 0 0

C B

for j = {k , i.e. if j = k 's di-

E =

C B

E j; k =

. . .

j;k

rect predecessor branch

C B

. . .

C B

0.10

A @

0 0 0

for j 6= {k , i.e. if j 6= k 's

direct predecessor branch

In this pap er 0 denotes a zero matrix whose dimension is apparent from the context. As a conse-

quence of the numb ering scheme used here, for j

th th

k branch and thus the j; k blo ck element, E j; k =0. Thus E is a strictly lower triangular

matrix. The analogs of equation 0.3 are as follows:

V = E V + H

= E + H + a

f = E f + M + b 0.11

T = Hf

= 0 and the op erator can be Once again analogous to equation 0.6, E is nilp otent E

de ned as follows:

1 2

=IE = I + E + E + + E 0.12

th 6 n 6n

It is easy to verify that the j; k blo ck element j; k 2 R of is given b elow:

for j = k =IE

j; k =

if 9 p>0: j ={ k; i.e., if j is a predecessor branchofk

j;k

0.13

0 if j 6= { k 8 p>0; i.e., if j is not a predecessor branchof k

where the comp onents of the matrix are de ned as

j;k

m; l =m ;l = m ;m 1m 1;m 2 l +1;l

j j j j j

j;k k k k

for m 2 1 n and l 2 1 n . m ;l is the transformation op erator from hinge l on the

j j

k k k

th th

k branch to hinge m on the j branch and is a generalization of the transformation op erator

i; j in equation 0.6 for serial chains. It is formed by sequentially comp osing all the individual

transformation op erators that lie on the unique path joining the two hinges. The numb ering scheme

used here ensures that will b e a lower triangular matrix. The structure of the E and op erators

for the example in Figure is given b elow:

0 1 1 0

E 0 0 0 0 0 0 0 0

B C C B

0 E 0 0 0 0 0 0 0

B C C B

E = ; and =

0 0 E 0 0 0 0 0 0

B C C B

E E 0 E 0 0 0

B C C B

4;1 4;2 4

@ A A @

0 0 E E E

5;1 5;2 5;3 5;4 5

5;3 5;4 5

The op erator has state transition prop erties analogous to the for serial chains, and as a con-

sequence, it can be used for high{level and concise description and analysis of the dynamics of

tree top ology systems as in equation 0.14 b elow. However, from the computational p ersp ective,

equations involving these op erators can always b e directly mapp ed into very ecient and recursive

computational algorithms. From equation 0.11 and equation 0.12 it follows that,

V = H

= H + a 0.14

f = M + b=M H + M a + b

T = Hf = HM H + HM a + b

4 4

= M + C; where M = HM H ; and C = HM a + b

NN N

M 2 R denotes the mass matrix for the tree system, and C 2 R is the vector of velo city

dep endent nonlinear Coriolis typ e terms. While neither M nor C are typically available, their 11

explicit computation is not required either for solving the forward dynamics of the system. Since

the complexity of the forward dynamics algorithm dep ends primarily on handling M and not C ,

we assume here for simplicity that C is explicitly computed from the state information. De ning

T = T C, the forward dynamics then requires the solution of

M = T 0.15

for the hinge accelerations .

It is noteworthy that the spatial op erator expressions for the mass matrix M and the Coriolis

vector C in equation 0.14 are identical to the corresp onding ones for serial chains in equation 0.7.

Moreover, key prop erties of the op erators remain une ected. Thus, imp ortant op erator results for

the alternative square Innovations factorization and the inversion of the mass matrix for serial

chain systems in [9] also hold for tree{top ology systems. Indeed, with only minor extensions, even

the O N recursive algorithms for the dynamics of serial chain systems extend directly to tree{

top ology systems. The discussion of these op erator results and algorithms is p ostp oned until after

the dynamics of closed{chain systems have b een discussed in the next section.

However, b efore pro ceeding to closed top ology systems, an expression is derived for the tree-

top ology Jacobian op erator. Given n p oints, denoted C 's, on the tree, the Jacobian op erator

6n N 6 n

^ ^ ^

C C

J 2 R de nes the mapping between and V , i.e., V = J , where V 2 R denotes the

vector of spatial velo cities at these p oints. If C is on b o dy m , then the spatial velo city at C is

k k

given by

V k = [Om ;C ]V m 0.16

j j

with O m ;C denoting the rigid b o dy transformation op erator from C to the p oint O m .

j j

k k

6n 6n

With the blo ck elements of B 2 R de ned as

[Om ;C ] if C 2 m body

k k

B m ;k= for k =1 n 0.17

j C

0 otherwise

it follows from equation 0.16 that

V = B V = B H ; i.e., J = B H 0.18

This gives an expression for the desired Jacobian op erator that will b e used b elow to deal with the

lo op closure constraints for closed top ology systems.

Closed Top ology Systems 12

This section describ es equations of motion for closed top ology multib o dy systems with

time{varying top ologies, constraints and constituent b o dies. The closed{chain system consists of:

a the primary tree-top ology subsystem, consisting of the ma jor part whose top ology and con-

stituent b o dies are the least time{variant.

b The secondary tree{top ology subsystem, whichismuch smaller, and consists of the part whose

top ology and constituent b o dies do change from time to time.

c The set of closure constraints and/or b oundary conditions b etween/within the primary and

secondary systems.

Note that the subsystems describ ed ab ove are in an order of increasing time{variation. As an exam-

ple, take the case of multiple rob ot manipulators interacting with each other and the environment

to p erform complex tasks. In this context, the manipulators by themselves should b e assigned to

the primary system since their internal structure do es not vary with time. On the other hand, the

task ob jects and the to ols vary from task to task and are assigned to the secondary system. The

closure constraints b etween the primary and secondary subsystems characterize those arising from

task related grasping, mating, to ol op eration etc., and b elong to the last category.

This decomp osition of the closed top ology system is a departure from the more traditional

approach see [10 , 11 ] of forming a spanning tree for the full system and computing the constraint

forces at the p oints of closure. In these latter approaches, even small changes in the original

system typically require whole new spanning trees for the system. This disallows any algorithmic

optimization, and the algorithms are also not amenable to coping with time{varying systems.

When the application system is \time{invariant", such as a spacecraft, there may b e no secondary

subsystem at all, and in the absence of closed lo ops, the set of constraints will also b e empty.

The equations of motion for tree top ology systems derived in equation 0.15 will b e used

to describ e the dynamics of the tree comp onents of b oth the primary and secondary systems, with

the subscripts \P " and \S " b eing used to identify the two subsystems. Thus the dynamics of the

tree part of the two systems are describ ed by

_ _ _ _

T = H M H = M ; and T = H M H = M

P P P P P P P S S S S S S S

P P S S

0.19

M and M denote the mass matrices, and the motion dof parameter vectors, T and T

P S P S P S

the bias{free internal hinge forces for the primary and secondary subsystems resp ectively. 13

Collect together the internal lo op p oints of closure with the p oints of closure coupling

the two systems, and denote their number by and for the primary and secondary systems

P S

6 6

^ ^

P S

resp ectively. Let V 2 R and V 2 R denote the vectors of spatial velo cities at these p oints of

P S

closure for the two systems. Following the discussion leading up to equation 0.18, J = B H

P P P

and J = B H are the Jacobians to these p oints of closure for the two systems resp ectively.

S S S

^ ^

Thus V = J and V = J . The kinematical constraints due to the existence of internal

P P P S S S

closed lo ops within the primary and secondary systems are characterized by constraint matrices

Q and Q and lead to kinematical constraint equations of the form:

P S

^ ^ ^ ^

V = U V = U and Q Q

S S P P S P

The coupling together of the primary and secondary systems is characterized by the constraint

^ ^

matrices Q and Q and leads to a kinematical constraint equation of the form:

P S

^ ^ ^ ^ ^

Q V + Q V = U

P P S S C

De ne

0 1 0 1

^ ^

Q Q

P S

B C B C

4 4 4

B C B C

A = ; A = ; and A =[A A ]

B C B C

Q 0

P S P S

@ A @ A

0 Q

The closure constraints can b e collectively expressed as:

1 0

0 1 0 10 1 0 1

C B

V J 0

P P P 4 P

C B

@ A @ A@ A @ A

A =[A A ] =[A J A J ] = = U

C B

P S P P S S

A @

V 0 J

S S S S

0.20

It is assumed here onwards that [A J A J ] is of full row rank, and its rank is N . The overall

P P S S E

number of motion dofs of the closed chain system is given by N = N + N N . While not

C S E

required for the purp ose here, equation 0.20 can be used, whenever necessary, to nd the N

dimensional minimal set of generalized velo cities for the system. Based on the principle of virtual

work, equation 0.20 implies that the closure constraint forces are

0 1

J A

P P

@ A

f 0.21

J A

S S

. Together, the dynamical equations of motion for the primary and secondary for some f 2 R

systems equation 0.7 and equation 0.21, as well as the constraint equation in equation 0.20,

lead to the following equations of motion for the closed chain system follows:

0 1 0 1 0 1

0 1

M 0 J A

P P P

P P

B C B C B C

h i

B C B C B C

_ _

@ A

= ; where U = U A J A J

B C B C B C

0 M J A T

P P S S

S S S

S S

@ A @ A @ A

0.22

A J A J 0 f U

P P S S 14

Premultiplication of b oth sides of the ab ove equation by the matrix

0 1

I 0 0

B C

0 I 0

@ A

1 1

A J M A J M I

P P S S

P S

leads to

1 1 0 0 1 0

T M 0 J A

P P P

P P

C C B B C B

0 M J A

S S S

S S

A A @ @ A @

0.23

1 1

U [A J M T + A J M T ] f 0 0 [A A + A A ]

P P P S S S P P S S

P S

where

4 4

1 1

= J M J ; and = J M J

P P S S

P S

6 6 6 6

P P S S

Physically, 2R and 2 R are the e ective \admittances" of the primary and

P S

secondary systems re ected to the p oints of closure.

When there is no secondary system, the equations of motion in equation 0.22 are given

0 1 0 1 0 1

M J A T

4 P P P

P P

@ A @ A @ A

= ; where U = U A J V

P P P

A J 0 f U

P P

The corresp onding form of equation 0.23 is given by:

0 1 0 1 0 1

M J A T

P P P

P P

@ A @ A @ A

= = 0.24

0 A A f U A J M T

P P P P P

Simpli cations for some sp ecial cases typ es of closed chain systems are describ ed b elow:

When the hinge constraints coupling the primary and secondary systems are only on the

relative spatial velo city across the hinges coupling them together, an appropriate reordering

^ ^ ^

of the elements of V will result in Q = Q . Furthermore, if no relative motion is p ermitted

P S

^ ^

across the hinge, i.e., there is rigid rather than lo ose coupling, then in fact Q = I and Q = I .

P S

When this feature holds for only some of the hinges, only the corresp onding rows of Q and

Q have this sp ecial structure.

When the secondary system has no internal actuators or source of generalized forces, then

T =0.

If the secondary system is a free rigid body with no internal degrees of freedom, then the

motion generalized co ordinates vector is of dimension 6 and is just the spatial velo cityof

the b o dy. 15

Forward Dynamics of Closed Chain Systems

This section describ es a recursive algorithm for solving the forward dynamics of closed chain

rigid multib o dy systems which do es not require the explicit computation of the system mass matrix.

From the equations of motion for closed chain systems given by equation 0.23, the forward

dynamics problem can b e solved by the following sequence of steps:

f f f f

_ _ _ _

A Solve M = T for Solve M = T for

P P S S

f f f f

_ _

B Compute = J Compute = J

P S

P P S S

1 1

C Compute = J M J Compute = J M J

P P S S

P S

f f

^ ^

D Solve [A A + A A ]f =A +A U for f

P P S S P S

P S

_ ^ _ _ ^ _

E Solve M = J A f for Solve M = J A f for

P S

P P P P S S S S

f f

______

F = + = +

P S

The interpretation of each of the steps is as follows. STEP A solves for the \free" hinge

f f

_ _

accelerations and of the primary and secondary systems assuming that there are no closure

P S

constraints on the system. Note that this step is equivalent to computing the forward dynamics of

the primary/secondary tree{top ology systems. STEP B computes the corresp onding \free" tip

f f

spatial accelerations and for the systems. STEP C computes the e ective admittances of

P S

the primary and secondary systems at the p oints of closure. In STEP D, the constraint forces at

the p oints of closure are computed, and they are used in STEP E to compute the \correction"

_ _

hinge accelerations and for the two systems. Combining the free and correction hinge

P S

accelerations in STEP F gives the true hinge accelerations for the two systems.

It is now easy to see the use of the decomp osition into primary and secondary systems

in the development of dynamics algorithms which are resp onsive and adaptable to time{varying

systems. The forward dynamics pro cedure ab ove involves a sequence of decoupled steps for each

of the primary and secondary system dynamics, and one step in which they come together when

the constraint forces are computed. Being structurally time{invariant, it is p ossible to put in place 16

optimized algorithms for the dynamics of the primary system. Changes in the secondary system,

which is typically of small dimension, e ect only the relatively smaller number of computations

involving the steps in the right half column. Changes in the closure constraints only alters the

constraint matrix A, and thus only STEP D is a ected, while the computations for the primary

and secondary systems remain una ected. Thus, changes to the secondary system or the constraints

on the system require very mo dest changes to recon gure the algorithm to resp ond to these changes.

Recursive algorithms for each of the ab ove steps are now describ ed. It follows from the

symmetry between the steps in the two columns that the recursive algorithms for the primary

system are directly applicable to the secondary system as well. Therefore, the explicit use of the

subscripts identifying the primary/secondary systems are dropp ed here onwards except for STEP

STEP A Solve M = T . Forward Dynamics of a Tree Top ology System

Step A is equivalent to solving the forward dynamics of a tree top ology system. Here, an

O N recursive algorithm for this solution is develop ed. This algorithm is based on a new

factorization of the mass matrix M in terms of square factors. in contrast to the earlier non{

square factorization in equation 0.14. This square factorization is then used to obtain an

explicit expression for M .

The articulated body inertia matrix P is de ned as the solution to the following equation:

M = P E [P PH HP H HP ]E 0.25

P is block diagonal and the elements on the diagonal denoted P k can b e obtained using

a recursive algorithm describ ed in equation .39 in App endix A. Physically, P k is the

articulated body inertia as seen at the k hinge, i.e., it is the e ective inertia of all the links

outb oard from the k hinge assuming that the hinge forces at all the outb oard hinges are zero.

For the subsequent development, it is convenient to de ne

4 4 4

D = HP H ; G = PH D ; K = E G

4 4 4

= GH ; = I ; E = E 0.26

Note that D ; G; and are all blo ck diagonal. The structure of E is identical to that of E

with its elements b eing given by

k ;k 1 = k ;k 1 k 1

j j j j j 17

E is also nilp otentE = 0, and analogous to , is de ned as

1 2 n1

=IE = I +E +E + + E 0.27

The structure of is very similar to that of and it also p ossesses the state transition prop erties

which are used to develop recursive algorithms. may b e viewed as the transformation op erator

for comp osite b o dies i.e., as if all the hinges are lo cked, while is the transformation op erator

for articulated b o dies i.e., as if all the hinge forces were zero. The following lemma yields a

square factorization of M.

Lemma 0.1 The mass matrix M has the fol lowing factorization:

M =[I +HK]D[I + HK] ; 0.28

Pro of: See App endix B.

Note that the factor [I + HK] is square and blo cklower triangular, while D is blo ck diagonal.

Thus the factorization in equation 0.28 may b e thought of as an explicit blo ck LD U factor-

ization of the mass matrix. This factorization is also known as the innovations factorization

b ecause of its relationship to the innovations approach to ltering and prediction theory see

[17 ].

The following lemma gives the explicit form for the inverse of [I + HK].

Lemma 0.2

[I + HK] =[I HK] 0.29

Pro of: See App endix B.

Combination of Lemma 0.1 and Lemma 0.2 leads to the following form for the inverse of the

mass matrix.

Lemma 0.3

1 1

M =[I HK] D [I HK] 0.30 18

Note that the factor [I HK] is square and blo ck lower triangular, while D is blo ck

diagonal. Thus the factorization in equation 0.29 may be thought of as an explicit blo ck

LD U factorization of the mass matrix inverse.

Thus,

f 1 1

= M T =[I HK] D [I HK]T 0.31

The O N recursive computation of the expression on the right is given in equation .40 in

App endix A.

f f

STEP B Compute = J

f f

From equation 0.18, = B ~ , where

f f

~ = H 0.32

However,

Lemma 0.4

I HKH = H 0.33

Pro of: See App endix B.

Thus, use of equation 0.30 and the ab ove lemma in equation 0.32,

f 1 1

~ = H [I HK] D [I HK]T = H D [I HK]T

Compare this with equation 0.31 to see that ~ can b e evaluated as an intermediate quantity

in the O N recursive algorithm for computing describ ed in STEP A.

STEP C Compute = J M J

Use of equation 0.18 and equation 0.30 implies

= f[I HK]HBg D f[I HK]HBg

1 1

= B H D HB = B B; where = H D H 0.34 19

Here equation 0.33 has b een used to simplify the ab ove expression. A recursive O N pro-

cedure for the computation of is given in equation .43 in App endix A. Note that without

the simpli cation resulting from the use of equation 0.33, the computation of would b e an

O N pro cess.

f f

^ ^

STEP D Solve [A A + A A ]f =A +A U for f

P S S P S

P S

Now,

f f

f =[A A +A A ] [A + A U ] 0.35

P S S P S

P S

In this form this step is of O N complexity. However, when A A is invertible, there

P P

E P

is an alternative expression for f . This is obtained by reexpressing equation 0.23 as:

0 1 1 1 0 0

M 0 J A

P P

B C C C B B

0 M J A

S S S

S S

@ A A A @ @

U A f 0 A J A A

P S S P P

Premultiply b oth sides by

0 1

I 0 0

B C

0 I J A A A

S P P

S P

@ A

0 0 I

to get

1 1 0 0

M 0 J A

P P

C C B B

0 M + J A A A A J 0

S S P S S

S S P

A A @ @

f 0 A J A A

S S P P

0 1

B C

T + J A A A [U A ]

S P P P

S S P

@ A

U A

From the ab ove equation it follows that

1 1 1

= [M + J A A A A J ] [T J A A A A U ]

S S P P S S S P P

S S P S S P

^ _

f = A A [A + A J U ]

P P S S S

= A A [A + A U ]; where = J

P P P S S S S S

Note the similaritybetween the forms of equation 0.35 and the ab ove equation for f . The

computational cost of the ab ove op eration is a combination of the cost of inverting A A ,

P P

P 20

and the O N step of solving a square linear system of equations of size N . The cost of

inverting A A dep ends on its structure: its sparsity re ects the degree of coupling b etween

P P

the closed lo ops in the system. The cost is typically much less than the worst case of O N .

In many application domains such as rob otics, A A is in fact blo ck diagonal and is thus

P P

invertible in O N steps [8 ]. In addition, for most applications N N , and this new

E S E

formulation can lead to considerable computational savings.

The inverse of [A A + A A ] will not exist if [A J A J ] is not of full rank, i.e.,

P P S S P P S S

P S

the con guration is such that the number of motion dofs for the system have changed. It is

therefore necessary to reformulate the constraint equation equation 0.20 so as to preserve the

full rank prop erty. Suchchanges of rank can o ccur at kinematically singular con gurations.

STEP E Compute = M J A f

From equation 0.30 and equation 0.18,

= [I HK] D [I HK]HBA f

Use of Lemma 0.4 in this leads to

= [I HK] D HBA f 0.36

The recursive O N implementation of the ab ove step is given in equation .44 in App endix

The overall complexity of this spatially recursive forward dynamics algorithm ranges be-

3 3

tween O N + N +ON for the worst case and O N + N +ON +ON in the b est

S S E

E S

case.

By treating the primary and secondary system as one overall system, which amounts to

4 4

de ning the quantities = diag ; , H = diag H ;H etc., and using the ab ove results, for

P S P S

U = 0 the overall closed top ology forward dynamics algorithm can be restated in the following

form:

h i

1 1 1

2 2 2

I bAA b D [I HK]T; where b = D HBA =[I HK] D

0.37

Note that when there are no closed lo ops in the overall system, A = 0, and the middle term reduces

to I . Hence, the forward dynamics of tree top ology systems in equation 0.31 are recovered. 21

REFERENCES REFERENCES

Conclusions

The algorithms develop ed here are suitable for multib o dy systems that have time{varying

top ology as well as changing constituent b o dies and constraints. For the sake of clarity, the fo cus

of much of the pap er is on multib o dy systems with rigid links.

This pap er describ es the use of the spatial operator algebra to easily develop these algo-

rithms. Based on the rate of time{variation, a multib o dy system is partitioned into a primary

subsystem, a secondary subsystem and the set of closure constraints. Spatial op erators are used to

develop op erator factorizations and inversion of the mass matrix. These factorizations lead directly

to ecient computational algorithms for the dynamics of the multib o dy system. The algorithm

consists of of parallel paths involving the tree{top ology primary and secondary systems resp ectively.

The two paths come together at one p oint to compute the constraint forces. The algorithm can

be adapted to time{varying top ology and changes in constraints or constituent b o dies since only

lo calized and relatively easy mo di cations to the algorithm are required.

The algorithm do es not require the computation of the mass matrix, and its computational

complexity is linear in the number of degrees of freedom, N + N . In the absence of kinemat-

P S

ical singularities, the algorithmic complexity also dep ends only linearly on the number of closure

constraint equations, N .

Reference [13 ] describ es extensions to exible multib o dy systems. The spatial op erator

formulation for exible multib o dy systems is identical in form to the formulation for rigid multib o dy

systems describ ed in this pap er. The structure of the dynamics algorithms requires the addition of

only a few straightforward steps to handle b o dy exibility.

Acknowledgement

The research describ ed in this pap er was p erformed at the Jet Propulsion Lab oratory,

California Institute of Technology, under contract with the National Aeronautics and Space Ad-

ministration. K. Kreutz{Delgado is also supp orted by an NSF PYI and a CalSpace grant.

References

[1] Vereshchagin, A.F., \Computer Simulation of the Dynamics of Complicated Mechanisms of

Rob ot-Manipulators," Engineering Cybernetics,vol. 6, pp. 65{70, 1974. 22

REFERENCES REFERENCES

[2] Featherstone, R., \The Calculation of Rob ot Dynamics using Articulated-Bo dy Inertias," The

International Journal of Robotics Research, vol. 2, pp. 13{30, Spring 1983.

[3] Ro driguez, G., \Kalman Filtering, Smo othing and Recursive Rob ot Arm Forward and Inverse

Dynamics," IEEE Journal of Robotics and Automation,vol. 3, pp. 624{639, Dec. 1987.

[4] Brandl, H., Johanni, R., and Otter, M., \A Very Ecient Algorithm for the Simula-

tion of Rob ots and similar Multib o dy Systems without Inversion of the Mass Matrix," in

IFAC/IFIP/IMACS Symposium 1st, Vienna, Austria, pp. 95{100, 1986.

[5] Bae, D.S. and Haug, E.J., \A Recursive Formulation for Constrained Mechanical System

Dynamics: Part I. Op en lo op systems," Mech. Struct. & Mach., vol. 15, no. 3, pp. 359{382,

1987.

[6] Rosenthal, D.E., \An Order n Formulation for Rob otic Systems," The Journal of the Astro-

nautical Sciences,vol. 38, no. 4, pp. 511{530, 1990.

[7] Ro driguez, G., \Kalman Filtering, Smo othing, and Top ological Tree Dynamics," in VPI/SU

Symposium on Control of Large Structures, Blacksburg, VA, June 1987.

[8] Ro driguez, G. and Kreutz-Delgado, K., \Recursive Mass Matrix Factorization and Inversion:

An Op erator Approach to Manipulator Forward Dynamics," 1992. see also JPL Publication

88-11, 1988.

[9] Ro driguez, G., Kreutz-Delgado, K., and Jain, A., \A Spatial Op erator Algebra for Manipulator

Mo deling and Control," The International Journal of Robotics Research,vol. 10, pp. 371{381,

Aug. 1991.

[10] Brandl, H., Johanni, R., and Otter, M., \An Algorithm for the Simulation of Multib o dy Sys-

tems with Kinematic Lo ops," in World Congress on the Theory of Machines and Mechanisms

7th, Seville, Spain, 1987.

[11] Bae, D.S. and Haug, E.J., \A Recursive Formulation for Constrained Mechanical System

Dynamics: Part I I. Closed lo op systems," Mech. Struct. & Mach.,vol. 15, no. 4, pp. 481{506,

1987{88.

[12] Bryson, A.E. , Jr and Ho, Y. C., Applied Optimal Control. Blaisdell Publishing Co., 1969.

[13] Jain, A. and Ro driguez, G., \Recursive Dynamics for Flexible Multib o dy Systems Using Spa-

tial Op erators," JPL Publication 90-26, Jet Propulsion Lab oratory,Pasadena, CA, Dec. 1990.

[14] Jain, A., \Uni ed Formulation of Dynamics for Serial Rigid Multib o dy Systems," Journal of

Guidance, Control and Dynamics,vol. 14, pp. 531{542, May{June 1991. 23

REFERENCES REFERENCES

[15] Kane, T.R. and Levinson, D., Dynamics: Theory and Applications. McGraw-Hill, New York,

1985.

[16] Ro driguez, G., \Random Field Estimation Approach to Rob ot Dynamics," IEEE Transactions

on Systems, Man and Cybernetics,vol. 20, pp. 1081{1093, Sept. 1990.

[17] Kailath, T., \The Innovations Approach to Detection and Estimation Theory," Proceedings of

the IEEE,vol. 58, pp. 680{695, Mar. 1970.

App endix A: Recursive Computational Algorithms

Based on the sp ecial structure of ; etc., it is p ossible to evaluate many of the dynamical

expressions in a recursive manner and we describ e some recursive algorithms in this app endix. First

we de ne some notational shorthand to simplify the description of these algorithm:

n +1 = 1

y1 ; 0 x0 = y 1 ;n xn .38

j j j j m m

m2{ j

y 1 ; 0 x0 y 1 ; 0 = y 1 ;n xn y 1 ;n

j j j j j j m m j m

m2{ j

where y :; : and x: stand for some appropriate arrays. Thus where ever in an algorithm, a term

with indices as in the left column app ears, its meaning is actually given by the corresp onding term

in the column on the right.

A recursive metho d for the computation of the blo ck diagonal elements of P as de ned by 24

REFERENCES REFERENCES

equation 0.25 and the entries of D , G, K , E and de ned in equation 0.26 is given by:

for j = 1 `

If { j =;, then P 0 =0

for k = 1 n

j j

Pk = k; k 1P k 1 k; k 1 + M k

D k = H k P k H k

.39

Gk = P k H k D k

k = I Gk H k

k +1;k = k +1;kk

Kk +1;k = k +1;kGk

end lo op

f 1

The recursive computation of =[I HK] D [I HK]T in equation 0.31 in STEP

A can be carried out via the O N tree top ology forward dynamics algorithm describ ed

f 1

b elow. It also results in the computation of = D [I HK]T required in STEP B 25

REFERENCES REFERENCES

as an intermediate quantity.

for j = 1 `

If { j =;, then z 0 =0;T0 =0

j j

for k = 1 n

j j

zk = k; k 1z k 1 + K k; k 1T k 1

k = T k H k z k

> k = D k k

end lo op

n +1= 0

for j = ` 1

for k = n 1

j j

.40

f f

k = k +1;k k +1+H kk

f f

k = k K k +1;k k +1

end lo op

STEP C requires the computation of = B B . In order to obtain a O N recursivescheme

for the computation of we rst de ne the matrix as the one satisfying the equation:

H D H =E E .41

as de ned ab ove is a blo ck diagonal matrix and its elements can b e computed recursively.

Wenow obtain the following decomp osition of .

Lemma .5

~ ~

=+ + .42

Pro of: See App endix B.

Noting that is strictly lower triangular, we can then recognize that as nothing but the

diagonal elements of . We now present a recursive scheme to compute the blo ck diagonal 26

REFERENCES REFERENCES

elements of and of .

n +1= 0

for j = ` 1

for k = n 1

j j

k = k +1;kk +1k +1;k+H kD kHk

k; k=k

for m = k 1 1

j >

k; m = m; k =k; m +1m +1;m

end lo op

The ab ove recursion yields the elements on the blo ck diagonal of . Since is symmetric,

the o {diagonal elements satisfy = , and can b e computed from the diagonal elements

j;l

l;j

as follows. for l 2 1 j 1 can b e obtained via the following recursivescheme:

l;j

if { l =j for some p>0

for k = n 1

j j

for m = n 1

l l

k; m= m; k = k; 1 1 ;m

j j

end lo op

.43

end lo op

else

= 0

> j;l

l;j

end if

The O N recursive implementation of = [I HK] D HBA f in equation 0.36 in 27

REFERENCES REFERENCES

STEP E is given b elow:

De ne x^ = BA f

for j = 1 `

If { j =;, then z 0 =0;x^0 =0

j j

for k = 1 n

j j

zk = k; k 1z k 1 + K k; k 1x ^ k 1

k = H k z k

> k = D k k

end lo op

n +1= 0

for j = ` 1

for k = n 1

j j

.44

k = k +1;k k +1+H kk

k = k K k +1;k k +1

end lo op

App endix B: Pro ofs of the Lemmas

In this app endix we give the pro ofs of the various lemmas. First we establish a few identities.

Lemma .6 P satis es the equation

M = P E PE .45

Pro of: It is easy to verify from the de nitions in equation 0.26 that

P =0 and thus P = P

Thus we can rewrite equation 0.25 in the form

M = P E [I PH D H]PE = P E P E = P E P E = P E PE

REFERENCES REFERENCES

Lemma .7 In the stacked notation we have that:

= E = E = I .46

~ ~

M = P + P + P .47

= I + KH

= I + K H .48

HM H = D .49

Pro of:

a From equation 0.27 we have that E = I . Substituting this into equation .46 gives

the result.

b Pre{ and p ost{multiplying equation .45 by and wehave that

~ ~ ~ ~ ~ ~

M =+IP+I P = P + P + P

c Wehave from equation 0.26 and equation 0.27 that

1 1 1

= I E = I E =I E +E = +E GH = + KH .50

Pre and p osy-multiplying this by gives the result.

d From equation 0.26

P H = PH so that P H =0 and thus E PH =0

Pre{ and p ost{multiplying equation .47 with H and H resp ectively and using the ab ove

fact in conjunction with equation .46 yields the result. 29

REFERENCES REFERENCES

Pro of of Lemma 0.1:

Wehave that

1 1

M = HM H = H M H

= H [I + K H ]M [I + K H ] H =[I +HK]HM H [I + HK]

= [I + HK]D[I + HK]

Wehave used equation .48 and equation .49 ab ove.

Pro of of Lemma 0.2:

Using the op erator identity in equation .50 along with a standard matrix identity, it follows

1 1

that I + AB = I AI + BA B,wehave that

1 1 1 1

[I + HK] = I H[I + K H ] K = I H K = I HK

Wehave used equation .48 to simplify the ab ove.

Pro of of Lemma 0.4:

Wehave that

[I HK]H = H HKH=H H I=H

Wehave used equation .48 in the ab ove for simpli cation.

Pro of of Lemma .5:

Using equation .41 and equation .46 wehave that

~ ~ ~ ~

= H D H = =+ + 30