DEVELOPMENT AND EVALUATION OF AN ORDER-N FORMULATION FOR
MULTI-FLEXIBLE BODY SPACE SYSTEMS
Tushar K. Ghosh L-3 Communications, STRATIS 1002, Gemini Avenue, Suite 200
Houston, TX 77058, USA E-mail: [email protected]
Leslie J. Quiocho Mail Code ER7
NASA Lyndon B. Johnson Space Center 2101 NASA Parkway
Houston, TX, 77058, USA E-mail: [email protected] KEYWORDS
Multibody dynamics, Rigid and flexible bodies, Tree topology, Constraints, Closed loops, Order-N. ABSTRACT
This paper presents development of a generic recursive Order-N algorithm for systems with rigid and flexible bodies, in tree or closed-loop topology, with N being the number of bodies of the system. Simulation results are presented for several test cases to verify and evaluate the performance of the code compared to an existing efficient dense mass matrix-based code. The comparison brought out situations where Order-N or mass matrix-based algorithms could be useful.
INTRODUCTION
The Software, Robotics and Simulation Division (SRSD) of NASA Lyndon B. Johnson Space Center provides math modeling and simulation in support of engineering analyses and crew training activities for the center. The division currently has an efficient generic multibody dynamics code based on a dense mass-matrix formulation, which is used for simulating systems involving on-orbit robotic manipulators such as the Canadian Space Agency-built Space Station Manipulator System (SSRMS). It is generally known that Order-N (O(N)) algorithms, which involve arithmetic operation counts of the order N, where N is the number of bodies, perform more efficiently for systems with large degrees of freedom, compared to mass matrix-based with operations of order N3. It was therefore decided to develop an O(N) simulation for SRSD to investigate applications where they may perform better. This development was performed in-house to allow maximum flexibility and control in different simulations. ALGORITHM DEVELOPMENT
There are several methods in the literature that may be used for developing an O(N) algorithm. References may be found in (Banerjee, 2003). The formulation presented here is based on algebraically putting together the following steps: (1) kinematic equations relating motions between consecutive joints, (2) equations of motion of a single body, rigid or flexible, (3) equations relating the total spatial force and active forces and moments at the joint and (4) constraint conditions.
Derivation of the Order-N algorithm is presented in the Appendix. The effect of motion-induced stiffness (Banerjee, 1993) in flexible bodies of the system has not been incorporated yet. Inter-body forces may cause this effect to be important in some cases even for slow motions typical of space systems.
CODE VERIFICATION
The Order-N code was verified against the existing mass matrix-based code for several test cases, which itself was verified against other simulations in the industry, including TREETOPS (Singh et. al.,1985).The mass-matrix algorithm has been in use for many facilities at the Johnson Space Center for many years. Results from the two implementations matched with high accuracy (i.e., within 1.0e-10 or better).
SIMULATION TEST CASES
The test cases are based on variations of the system shown in Figure 1. The base plate B0 is a rigid circular plate floating in inertial space.
Leg1 (B1to B7) Leg 2 (B9to B16) Leg 3 (B17to B24) Base Plate, B0 Top Plate, B25 B1 B6 B9 B16 B17 Pitch Pitch Yaw Roll B2 B3 B4 B5
Legs 1 2 and 3 have identical mass properties Manipulators 1 and 2 have identical mass properties
B7 B8 Pitch Roll Yaw Pitch Pitch Pitch Yaw Roll Manipulator 1 (B26-B33) Manipulator 2 (B34to B41) Test Object, B42 Roll Yaw
Figures 1: Test Model Description
Three identical articulating legs and two identical manipulator arms are rigidly attached to the top plate. The other ends of the legs are rigidly connected to the base plate and the other ends of the manipulators hold a test object rigidly. All links are modeled as cylindrical rods. All joints of the legs and manipulators are single axis rotational joints. Two configurations of the legs and manipulators are considered, one with six joints the other with seven. The axes of the seven jointed manipulators and legs are in the order roll, yaw, pitch, pitch, pitch, yaw
and roll. The axes for ones with six joints are in the order roll, yaw, pitch, pitch, yaw and roll. In several configurations the boom elements of the legs have been split into two parts of equal length and joined rigidly, for adding additional bodies and flex degrees of freedom to the system. Only the boom elements are modeled as flexible. Each flexible rod has four bending modes (two in each of the bending planes). The system was driven by forces and moments on the top plate and the test object, and moments on joints of the first leg. The forces and moments were held constant for every 20 seconds and then switched in sign. Table 1 shows the configurations. Open loop cases are obtained by freeing the joints between legs 2, 3 and top plate and between manipulator 2 and the test object. For closed loop simulations constraint forces were determined at these same points. Table 1: System Test Case Configurations
RESULTS
Figures 2: Displacements of Top Plate and Test Object
Plots of representative data are shown in Figures 2 and 3. Figure 2 shows a co-plot of displacements of the center of the top plate, and the joint between manipulator 1 and the test object obtained from O(N) and mass matrix simulations for Case 16. The two results match to within 1.0e-10. Figure 3 is a co-plot of angular acceleration of the bottom pitch joint of leg 2 for Cases 12 and 16 (rigid and flex respectively) for O(N). Distances are shown in meters, while angles are shown in radians.
Figures 3: Leg 2 Bottom Pitch Joint Angular Acceleration Performance of the O(N) code is measured by the CPU time for a 100 second simulation and comparing it with the existing code. The results of the comparison are listed below for rigid and flexible models separately for both open-loop and closed-loop scenarios. Rigid cases were run with 0.001 second and flexible cases were run with 0.0001 second integration time step. An Euler-Cromer integration scheme was used.
Table 2: Timing Results
Ca se N u m b er Rigid (R) or F lex (F) Simul ation Confi g ura tio n ( T able 1 ) Open ( O ) or Clo se d (C) loop Degrees of Freedom CPU Time (sec) Rigi d Flex Tota l Order-N Ma ss Ma tr ix 1 R 1 O 24 0 24 10.1 6.7 2 R 2 O 24 0 24 12.4 7.9 3 R 3 O 27 0 27 15.0 10.1 4 R 4 O 42 0 42 22.3 18.6 S y st em Co nf ig ura tio n Legs Ma nipu la tors To tal N u mbe r of Bo d ie s Nu mb er o f Bo di es Nu mb er o f Jo in ts Bo om C ons tr uc ti o n Nu mb er o f bod ie s Nu mb er o f Jo ints 1 6 6 One rod 0 0 20 2 8 6 Two rods 0 0 26 3 10 7 Two rods 0 0 32 4 10 7 Two rods 8 7 49
5 F 1 O 24 24 48 166.8 174.0 6 F 2 O 24 48 72 277.9 423.6 7 F 3 O 27 48 75 275.9 478.0 8 F 4 O 42 80 112 408.6 1127.9 9 R 1 C 24 0 24 40.7 17.1 10 R 2 C 24 0 24 48.0 18.4 11 R 3 C 27 0 27 59.4 22.7 12 R 4 C 42 0 42 133.8 57.4 13 F 1 C 24 24 48 613.4 567.6 14 F 2 C 24 48 72 937.2 1402.0 15 F 3 C 27 48 75 1037.9 1551.2 16 F 4 C 42 80 112 2257.3 4063.9 Cases 9, 10, 11, 13, 14 and 15 have two loops, cases 12 and 16 have three loops.
DISCUSSION AND CONCLUSIONS
The timing results confirm that the dense mass matrix formulation is faster than the O(N) formulation for smaller degrees of freedom (DOF’s) and O(N) is faster otherwise. In our cases for all rigid systems mass-matrix take less CPU time than O(N) for both open-loop and closed-loop systems because they have fewer DOFs. Flexible body systems have more DOF and O(N) run faster, except for the case 13 involving two loops and fewer DOF than the other flex cases. Loops added large amount of computation and more so for O(N). For systems with loops the advantage of O(N) is reduced. With more loops the reduction is more because it requires solution of coupled linear constraint equations which can have a large number of unknown constraint forces and moments, and are solved in the usual manner that requires
3 c
n arithmetic operations, where n is the number of c
constrained degrees of freedom.
The high degree of match between the mass-matrix and O(N) results is expected because the two methods solve the same set of equations using many common codes.
ACKNOWLEDGMENT
The authors acknowledge the support provided by An Huynh and Thomas Brain of METECS Corporation, Houston Texas, USA in coding the simulation, and to Arun Banerjee, formerly of Lockheed Martin Palo Alto Research Laboratory, California, USA for review of the manuscript.
REFERENCES
Banerjee, A. K, “Block-Diagonal Equations for Multibody Elastodynamics with Geometric Stiffness and Constraints”, Journal of Guidance, Control and Dynamics, Vol. 16, No. 6, 1993. Banerjee, A. K., “Contributions of Multibody Dynamics to Space Flight: A Brief Review”, Journal of Guidance, Control and Dynamics, Vol. 26, No. 3, 2003.
Quicho, L,J.; 2010, T. Ghosh, D. frenkel, A. Huynh,
“Mode Selection Techniques in Variable Mass Flexible Modeling”, In AIAA Meeting Papers AIAA Modeling and Simulation Technologies Conference, Aug 2010, Toronto, Canada, AIAA- 2010-7606.
Singh, R.P., van der Voort, R.J., and Likins, P.W., “Dynamics of Flexible Bodies in Tree Topology-A Computer Oriented Approach”, Journal of Guidance, Control and Dynamics, Vol.26, No. 3, 1985.
APPENDIX: DERIVATION OF O(N) EQUATIONS System Description and Definitions
Figures A1: Multibody System Definitions
Figures A2: Constrained Motion Definitions Consider the set of rigid and/or flexible bodies, in Figure A1, with the associated labeling of forces and points.
n
R and
i
kˆ
R are frames attached to body n (B ) at its n
inboard and outboard joints On and
i
kˆ
O respectively. Figure A2 shows the labeling for constrained systems. Throughout this derivation it is assumed that all the vectors and inertia matrices are either generated in or are converted to a single reference frame, which is the structural reference frameR of the base body of the 0 multibody system. The conversion is performed every time the system states are updated.
Kinematics of Motion between Opand On
Letvn,an represent the inertial velocity and acceleration respectively, of On, represent the inertial angular n
velocity of R and n prepresent the flexible variables of
the body Bp. Kinematic equations relating position, velocity and acceleration ofOpand Onˆ, and angular velocity and angular acceleration of frames Rpand
nˆ R are given by p nˆ nˆ nˆ r , vnˆ vp~prnnˆp, p nˆ p nˆ n nˆ p n nˆ p nˆ ~ p nˆ p n p p p nˆ p n p nˆ a r~ ~ ~ r 2~ a p
is the undeformed value of rp, the vector fromOpto
nˆ
O . Combining the last two equations we can write
r , nˆ n nˆ p p , nˆ nˆ A S A A F (1.1) where, nˆ nˆ nˆ a A , 1 0 r~ 1 nˆ nˆ , n F , nˆ nˆ n S and n n , O p p nˆ p nˆ p p r , nˆ ~ ~ 2 r ~ ~ A .
F denotes a shift operator corresponding to an offset between two points identified by the subscripts. In this case the offset isrnˆ. The tilde (~) is the usual cross product operator on a vector.
The joint n may have up to six DOFs allowing both translation and rotation. The translation of Onwith
respect toOnˆis given by n n NT 1 i n , i n , i n gˆ G y n
where n , igˆ are linearly independent unit vectors fixed in Onˆ,
3
NTn is the number of translational degrees of freedom of joint n and are scalar quantities, i,n
representing joint translations in the directions of the unit vectors. The inertial acceleration of Onis:
ry n n n nˆ n nˆ n a ~y G a a (1.2) where, aryn ~nˆ~nˆyn2~nˆGnn Inertial angular velocity of R is given by n
NRn 1 i n , i n , i nˆ n lˆ = n n nˆL where lˆi,n is the unit vector in the direction of the rotation and i,n NRnis the number of rotational degrees
of freedom of the joint. L is a matrix whose columns are n the unit vectors lˆi,nand is a column matrix containing n
the rotations . Differentiating i,n in the inertial frame n
we get r n n n nˆ n L (1.3) n n r n L
Combining Equations (1.2) and (1.3),
rj n n n nˆ nˆ , n n A P A A F (1.4) n n n a A , n n n , r n ry n rj n a A 1 0 y ~ 1 n nˆ , n F and n n n L 0 0 G P (1.5)
Finally, combining Equations (1.1) and (1.4) we get
r n n n p n p p , n n A S P A A F (1.6)
where, using the relationship rnrnˆyn,
p , nˆ nˆ , n p , n F F F , SnFn,nˆSnˆ, ArnFn,nˆAnˆrAnrj
Inter-Body and Joint Actuator Forces
LetFnbe the spatial forces exerted by Bp on Bnat n
O and Fˆnˆbe the spatial force exerted by B on n Bpat
nˆ
O . Then from equilibrium considerations,
n T nˆ , n nˆ F Fˆ F (2.1) nˆ n,
F is given by Equation (1.5). Let and i,n be j,n
respectively, the i-th actuator force (i3)and j-th actuator torque (j3)on body n at On. They act in the directions
of the degrees of freedom gˆi,n and lˆj,nof the joint.
Defining n n
n as the array of actuator forces and
moments at joint n, it is straight forward to show that nPnTFn (2.2)
where P is given by Equation (1.5). n
Equations of Motion of Body n
A minor modification of equations of a flexible body in Quiocho, et. al (Quiocho, 2010), produces the equations of a body B in rigid and flexible coordinates as n
n , r i n , i , e T n , i n n , re n n , rr A M q F B M
F
i n , e n , i , e T n , i n n , ee n n , ee n n , ee n n , er B F S q D q K q M A M We shall write the equations of motion of the body n after separating the external spatial forcesFe,i,non Bnas (i)
from the previous body acting at the inboard joint (Fn),
(ii) from child bodies Bkiacting at outboard joints (
i
kˆ
Fˆ ), (iii) forces at constrained points (Fc,j) when there are such points on the body, and (iv) external forces (Fext,i,n) acting on the body. Because we chose the reference frame of the body to be at the inboard joint, the shift function at the inboard joint is an identity matrix and the shape/slope function at the inboard joint is a null matrix. Equations for body n are:
n , r i n , i , ext T n , i j , c ) n ( Z j T j , c i kˆ T n , kˆ n n n , re n n , rr B F F Fˆ F q M A M c i i
F F F
i n , e n , i , ext T n , i ) n ( Z j j , c T j , c i kˆ T kˆ n n , ee n n , ee n n , ee n n , er B F S F S Fˆ S q D q K q M A M c i i In the above equations F is the spatial force at the c,j
j-th constrained point,Fc,jis the shift function for the offset of the constrained point fromO and n Sc,jis the
shape/slope function of body n at the constrained point. Defining n , i , ext i T n , i n , r F G
F +Br,n (3.1) n n , ee n n , ee n , e n , i , ext i T n , i n , e S F B K q D q G
(3.2)Using Equations (2.1) for
i
kˆ
Fˆ and then Equations (3.1) and (3.2) we get the equations of motion of the body n as
n , r ) n ( Z j j , c T j , c i k T n , k n n n , re n n , rr G F F F q M A M c i i
F F (3.3) n , e ) n ( Z j j , c T j , c i k T k n n , ee n n , er A M q S F S F G M c i i
(3.4) Here, Zc(n)is the set of indices for constrained points on body n.Recursive Solution of Equations of Motion
The dynamical equations of motion of the system are solved recursively in steps as follows.
Step 1. Forward Pass Kinematics: In this step all position and velocity states are generated starting with the base body using equations derived in Kinematics section. Step 2. Inward Pass Dynamics Equations:
Observing the equations of motion it is reasonable to expect that for any n Fncan be expressed as linear functions of Ap, qpand constraint forces on itself and on bodies on outer branches:
j , c ) n ( Y j j , n , c n p n , q p n , A n D A D q d D F F c
(4.1) ) n (Yc is the set of indices of all constrained points on
body n and on all bodies of its outer branches.
We shall determine the coefficients in the above equations recursively. In Euation (4.1) using ki in place of n and n in place of p, and using the result in Equation (4.1) we get
) n ( Y j j , c j , n , c n n n , A n c F Qˆ qˆ A Qˆ q (4.2) n , er 1 n , ee n , A Mˆ Mˆ Qˆ ,
) n ( Z i k , q T k n , ee n , ee M S iD i Mˆ
) n ( Z i k , A T k n , er n , er M S iD i Mˆ
) n ( Z i k T k n , e 1 n , ee n M [G S d ] qˆ i i j , k , c T k n , ee j , n , c Mˆ S iD i Qˆ for iZ(n), jYc(ki) T j , c n , ee j , n , c Mˆ S Qˆ for jZc(n) ) n (Z is the set of indices of child bodies of B . In the n
same manner, substituting for F in Eq. (3.3) and ki
rearranging we get,
) n ( Y j j , c j , n , c n , r n n n , re n n , rr c F D G F q M A M (4.3) i i A,k ) n ( Z i T n , k n , rr n , rr M D M
F , i i q,k ) n ( Z i T n , k n , re n , re M D M
F , i i k ) n ( Z i T n , k n , r n , r G d G
F ` j , k , c T n , k j , n , c i D i D F for iZ(n), jYc(ki) T j , c j , n , c D F for jZc(n)Using Equation (4.2) for q in Equation (4.3) we get n
) n ( Y j j , c j , n , c n , r n n n , rr c F Dˆ Gˆ F A Mˆ (4.4) n , A n , re n , rr n , rr M M Qˆ Mˆ , Gˆr,nGr,nMre,nqˆn j , k , c n , re j , n , c j , n , c D M Qˆ i Dˆ Using Equation (1.6) for Anin Equation (4.4) and rearranging, we get
) n ( Y j j , c j , n , c R , n n , rr n , r n n n n , rr p p , n n , rr p p , n n , rr c F Dˆ A Mˆ Gˆ F P Mˆ q S Mˆ A Mˆ F Pre-multiplying this equation byP , using nT
n T n
nP F
from Equation (2.2) and solving the resulting equation for we get n
j , c ) n ( Y j j , n , c n p n , q p n , A n B A B q b B F c
(4.5) p , n n , rr T n 1 n , n , A M P Mˆ B F , p , n n , rr T n 1 n , n , q M P Mˆ S B ,
P (Gˆ Mˆ A )
M bn 1,nn nT r,n rr,n n,R , j , n , c T n 1 n , j , n , c M P Dˆ B , and M,n PnTMˆrr,nPnEquation (4.5) for in Equation (1.6) yields for n A n j , c ) n ( Y j j , n , c n p n , q p n , A n W A W q w W F A c
(4.6)n , A n p , n n , A P B W F ,Wq,nSn,pPnBq,n, R , n n n n Pb A w , and Wc,n,jPnBc,n,j
Equation (4.6) for A in Equation (4.2) gives n
j , c ) n ( Y j j , n , c n p n , q p n , A n Q A Q q q Q F q c
(4.7) n , A n , A n , A Qˆ W Q ,Qq,nQˆnWq,n, qˆnQˆA,nwnqˆn, and Qc,n,jQˆA,nWc,n,jQˆc,n,jFinally, using Equation (4.6) in Equation (4.4) we get
j , c ) n ( Y j j , n , c n p n , q p n , A n D A D q d D F F c
(4.8) n , A n , rr n , A Mˆ W D , Dq,nMˆrr,nWq,n n , r n n , rr n Mˆ w Gˆ d , and j , n , c j , n , c n , rr j , n , c Mˆ W Dˆ D We can see that Equation (4.8) is in the same form as Equation (4.1) we started with, confirming that if the latter is true for child bodies, it would be true for the current body also. Following the same steps as above for any outermost body for which F 0
i
kˆ it is easy to show
that Equation (5.1) is true for such bodies. Using the induction logic it therefore follows that Equation (4.1) is true for all bodies of the system.
Computations for systems with multiple branches need to start from an outermost body, move inward till a body with multiple child bodies is reached. Inward computation from a body with multiple child bodies should continue only after computations for all of its child bodies are completed.
0
A and F0 for the Base Body B0in terms of the constraint forces are obtained from Equation (4.4). When
0
B is fixed in inertial frameA0 and we then have 0
) 0 ( Y j j , c j . 0 , c 0 , r 0 c F Dˆ Gˆ FWhen B0is free in the inertial frame F0 and 0
) 0 ( Y j j , c j . 0 , c 0 , r 1 0 , rr 0 c ] F Dˆ Gˆ [ Mˆ A (4.9)The flexible coordinate acceleration q for the base body 0 is obtained from Equation (4.2)
j , c ) n ( Y j j , n , c 0 0 0 , A 0 Qˆ A qˆ Qˆ F q c
(4.10)For systems without closed loops the Fc,jterm drops out and the equations obtained above are sufficient for determining A0 and q , and then the system 0 accelerations, by successive computation of , n q and n
n
A in an outward sweep.
Step 3. Accelerations of Constrained Points in terms of Forces at Constrained Points:
For the determination of acceleration of constrained points in terms of forces at these points we seek to express the accelerationsA and n q for bodies in the path n from base body to the constrained points in the form
j , c A j , n A n n h H F A
and qn,j c,j q n n h H F q
(4.11) Here the summation index j covers all constrained points. It follows from Equations (4.9) and (4.11) that when the base is free in inertial frame0 , r 1 0 , rr A 0 Mˆ Gˆ h ,HA0,jMˆrr1,0Dˆc,0,j, hq0QˆA,0h0Aqˆ0 andHq0Qˆc,0,j.
When the base is fixed, h0A ,0 H0A,j , 0 hq0qˆ0 and
j , 0 , c q 0 Qˆ H .
Using Equations (4.7) and (4.11) we get the recursive equations for hqnandHqn,j:
n q p n , q A p n , A q n Q h Q h q h and j , n , c q j , p n , q A j , p n , A q j , n Q H Q H Q H
Using Equation (4.6) and (4.11) we get the recursive equations for hAn and HAn,j:
n q p n , q A p n , A A n W h W h w h , and j , n , c q j , p n , q A j , p n , A q j , n W H W H W H .
Let Eibe the point in constraint i located on body n and
' i
E be the mating point of the same constraint, located on body
. The spatial acceleration of E is ir , E n n , E n n , E Ei i A S i q A i A F (4.12)
where FEi,nis the standard shift operator for the offset
i E r of i E with respect to On i i i E E n , E S is the
shape/slope function of Bnat Ei and,
n E n n E E n n r , E ~ q ) q 2 r ~ ( ~ A i i i i .
For each constraint k (k = 1, 2, …,
n
c) the constraint forces Fc,kand Fc,k' at the two constrained points Ekand Ek' respectively are equal and opposite, i.e,
k , c ' k , c F
F . Using this and Equation (4.11) for Anand n q in Equation (4.12) we get, k , c q ' k , n q k , n n , E A ' k , n n 1 k A k , n n , E r , E q n n , E A n n , E E F )] H H ( S ) H H ( [ A h S h A i c i i i i i
F F (4.13) Similarly, spatial acceleration ofE
i'is given byk , c q ' k , q k , , E A ' k , n 1 k A k , , E r , E q , E A n , E E F )] H H ( S ) H H ( [ A h S h A ' i c ' i ' i ' i ' i ' i
F F(4.14) Step 4. Determination of Constraint Forces:
Using equations (4.13) and (4.14) the difference in the accelerations at the constraint i may be written as
k , c k k , E E E E E A A h H F A i i i' i
i (4.15) r , E q n n , E A n n , E r , E q , E A , E E i i i ' i ' i ' i i A h S h A h S h h F F ) H H ( S ) H H ( S ) H H ( ) H H ( H q ' k , n q k , n n , E q ' k , q k , , E A ' k , n A k , n n , E A ' k , A k , , E k , E i ' i i ' i i F FThe summation range for k is all the constraints. The linear and angular constraints at E are i
0 ) v v ( gˆc,i,j Ei'Ei for 0jnt,i 0 ) ( ˆ i ' i E E j , i , c for 0jnr,i
where vand represent the inertial velocity and angular velocity of points corresponding to the subscripts,
j , i , c
gˆ and ˆc,i,jare unit vectors in the direction of translational and rotational constraints respectively, and
) 3 (
nt,i and nr,i(3)are the number of these
constraints, respectively. i E v , ' i E v , i E and i E are determined in the manner used for the point Onˆ in the Kinematics section,. Defining
i ' i i ' i i E E E E E v v V and i , c i , c i , c 0 L 0 G P where i , c
G is a 3nt,imatrix whose columns are the unit
vectors gˆc,i,jand Lc,iis a 3nr,imatrix whose columns
are the unit vectors ˆc,i,j, the difference in the spatial
velocities of the constrained points at constraint i may be written as i E T i , c V
P . The difference in the spatial
acceleration in the constraint directions at the constraint point should be zero, giving
0 V P A
PcT,i EicT,i Ei
Using Baumgarte’s stabilization scheme to limit constraint violation caused by numerical errors, this equation is modified to 0 V P C X P K V P A PcT,i EicT,i Ei i cT,i Ei i cT,i Ei i
K and Ciare positive constants to provide constraint stabilization. Using Equation (4.15) for
i E A we have i i i i i E T i , c i E T i , c i E T i , c E T i , c k k , c k , E T i , c V P C X P K V P h P F H P
(4.16) Let us define fc,k,jto be the constraint force in the direction gˆc,k,jand c,k,jthe constraint torque in the direction ˆc,k,jand k , c k , c k , c fFˆ . The force Fc,kat the
constraint point E due to forces and moments in the k
constraint directions is then given by Pc,kFˆc,k. Let
k , f k , f k , f 0 L 0 G
P where Gf,kand Lf,kare made of
the unit vectors normal to the constraint directions for translation and rotation andFˆf,kbe the array of forces and
moments in these directions. Net spatial force at Ekis k , f k , f k , c k , c k , c P Fˆ P Fˆ F (4.17)
Restricting to cases where Fˆf,kis fully known, we use k , f k , f k , c k , c k , c P Fˆ P Fˆ F in Equation (4.17) to get i , c k k , c k , c k , E T i , c H P Fˆ Z P
i (4.18) i i i i i E T i , c i E T i , c i E T i , c E k k , f k , f k , E T i , c i , c V P C X P K V P ] h Fˆ P H [ P Z
(4.19) Stacking Equation (4.18) for all constraints we getc c
cFˆ Z
M (4.20)
where the [i,k]submatrix of matrix Mcis given by k , c k , E T i , c k , i , c P H P M i
and Z is made of arrays c Zc,igiven by Equation (4.19).
Equation (4.20) is solved for Fˆcand the spatial forces
k , c
F at constraint points are found using Equation (4.17).
Step 5. Computation of System Accelerations:
After determination of forces at the constrained points,A and 0 are determined using Equations (4.9) 0 and (4.10). , n q , and n Anare determined recursively in
a forward pass using Equations (4.5), (4.7) and (1.6) respectively.
