Automatic Generation and Integration of Equations of Motion for Linked Mechanical Systems

Transcription

1 Automatic Generation and Integration of Equations of Motion for Linked Mechanical Systems D. Todd Griffith a, John L. Junkins a, and James D. Turner b a Deartment of Aerosace Engineering, Texas A&M University, Texas, USA b Adjunct Faculty, Deartment of Aerosace Engineering, Texas A&M University and President Amdyn Systems, White GA, USA Abstract This aer resents a new method for the automatic generation and integration of equations of motion using oerator overloading techniques. An automatic differentiation tool (OCEA: Object Oriented Coordinate Embedding Method) is utilized to comute the artial derivatives required for imlementing the traditional Lagrangian equations of motion. The user only builds the kinetic and otential energy. Unlike symbolic aroaches, the OCEA-Lagrangian method builds the model on-the-fly, no comlex re-rocessing rogram must be develoed for imlementing Lagrange s method. The new method combines the best of the symbolic and numerical equation of motion generation methods, while retaining the simlicity and elegance of the original Lagrangian method. The method is demonstrated for constrained and unconstrained linked flexible body systems. Introduction In industry, research, and the classroom, the study of dynamical systems by the generation and integration of equations of motion is a rite of assage for scientists and engineers. For all but the simlest roblems, however, this task can be laborious and error-rone. Real-world alications requiring engineering-level fidelity models can take man-months to man-years of effort to develo and validate by hand. The most common energy-based equation of motion generation method consists of Lagrange s method. There are two aroaches for develoing comuterbased multibody dynamics models: () Symbolic methods that use comuter symbol maniulation algorithms, and () Numerical codes that build the system kinematic and acceleration equations on-the-fly. Symbolic maniulation engines such as MAPLE, MATHEMATICA, or MATHCAD are commercially available and theoretically caable of imlementing Lagrange s method. In ractice, these comuter symbol maniulating systems are tasked for carrying out the required differentiations of the scalar Lagrangian function. A serious limitation of this aroach is that a model or engineering design changes corrut the assumed model for the system Lagrangian and force extensive recalculations for all of the system artial derivatives. A further drawback of symbolic-based methods is that the resulting symbolic files can be huge and the resulting software is generally only machine-readable. Purely numerically based algorithms imlement a generic solution algorithm for linking systems of rigid bodies that can undergo large and raid relative motions. Examles of

2 commercially available tools for the automated generation and integration of equations of motion include DADS, DISCOS, and ADAMS. Though owerful and broadly alicable, these tools use rigid algorithms and coordinate selections and are generally not amenable for introducing aroximations or testing advanced solution algorithms. It is very difficult to generate linearized equations of motion or artial derivative models for the urely numerically based class of algorithms. This aer resents a new method for the automatic generation and integration of equations of motion using oerator overloading techniques for overcoming the limitations of both the symbolic and numerical multibody modeling and simulation tools, [9,0]. The new method combines the best of the symbolic and numerical equation of motion generation methods, while retaining the simlicity and elegance of the original Lagrangian method, [5,9]. An added benefit of the oerator overloaded aroach is that new aroximation strategies and comutational algorithms are easily introduced and evaluated. Overview of OCEA and automatic differentiation This aroach utilizes the reviously develoed OCEA (Object Oriented Coordinate Embedding Method) extension for FORTRAN90 (F90), [9,0]. The OCEA ackage is an object-oriented automatic differentiation equation maniulation ackage. Comuter imlementation of differentiation is tyically accomlished by two distinct aroaches automatic differentiation and symbolic differentiation, [4]. The rimary distinction of these aroaches is that automatic differentiation invokes the chain automatically and takes lace in the background. This allows for time and memory otimized differentiation. Symbolic differentiation allows the analyst to view the exressions; however, it requires more user intervention. Tyically, derivatives are comuted using a symbolic differentiation rogram, such as Macsyma or Mathcad, and the derivatives then need to be either hand tyed in a comuter rogram or saved in a file that tyically requires editing and comilation. OCEA defines embedded variables that reresent abstract data tyes, where hidden dimensions (background arrays) are used for storing and maniulating artial derivative calculations. OCEA relaces each scalar variable in the roblem with a differential n-tule consisting of the following variables (second-order OCEA method): f : = f f f () where and denote symmetric first- and second-order gradient tensors with resect to a user-defined set of indeendent variables. The introduction of the abstract differential n-tule allows the comuter to continue to maniulate each scalar variable as a conventional scalar variable, even though the first and higherorder artial derivative are attached to the scalar variable in a hidden way. The individual objects are extracted, using OCEA s adoted notation, as follows: f = f % E, f = f % V, and f = f% T. The automatic comutation of the

3 artial derivatives is achieved by oerator-overloading methodologies that redefine the intrinsic mathematical oerators and functions using the rules of calculus. For examle, addition and multilication are redefined as follows. () a* b: = a* b ( a* b) ( a* b i j i ) (3) a+ b: = a+ b a+ b a+ b Thus the + and * oerators are overloaded so that coding the left side exressions of Eqs. () and (3) causes all the right side comutations to be carried out. More subtly, if z = a+ band z = a* b, then comuting z = z + z causes the 3 results of Eqs. () and (3) to be roagated efficiently in the background to comute z = * ( 3 a + b + a b i z3) j i( z3). Additional oerations for the standard mathematical library functions, such as trigonometric and exonential functions, are redefined to account for the known rules of differentiation. In essence, this aroach re-codes, once and for all, all of the artial derivatives required for any roblem, and the chain rule is imlemented automatically in background oerations that the user neither derives nor codes. At comile time, and without user intervention, the OCEA-based aroach links the subroutines and functions required for, for examle, evaluating the system Lagrangian and its associated artial derivative models. Unlike conventional numerically based multibody codes, the resulting equations of motion are not hardcoded. Also, the exected comutational efficiency is similar to a manually generated set of equations of motion and simulation code, because the entire oerator-overloaded artial derivative calculation is otimally re-defined at comile time. Equations of Motion formulations For many decades, a rimary focus of analytical dynamics has been the develoment of methods for generating equations of motion. The classical formulations include Newton/Euler methods, D Alembert s equations, the Lagrangian energy aroach, and Hamiltonian aroaches, [,6,7]. More recent aroaches have been develoed in the ast century including Kane s equations, the Gibbs-Aell equations, and the Boltzmann/Hammel equations. Each of these methods have secific advantages and shortcomings when it comes to imlementing them in multibody dynamics codes. An overview of some of these issues is given in []. One major result of this aer is a demonstration of the use of the automatic differentiation caability of OCEA to generate and simultaneously comute solutions of the equations of motion. The most obvious dynamic formulation to utilize automatic differentiation is Lagrange s equations. This aer reresents a

4 ste in a new direction in the automatic formulation and solution of equations of motion. The end goal of this work is to model and simulate the behavior of linked mechanical systems. Secial emhasis is given to the formulation of the equations of motion. This aer is an extension of revious work [5], which demonstrated solutions by this method for linked rigid body systems, and is summarized as follows: () automatic generation of equations of motion via Lagrange s Method, () a generalization of the kinetic energy exression for multile flexible bodies, and (3) solutions for oen- and closed-loo chains of flexible body systems. Equations of Motion via automatic differentiation of the Lagrangian function In the Lagrangian formulation, artial derivatives of energy functions are utilized to roduce the equations of motion. An obvious advantage of the Lagrangian formulation over, for examle, Newton/Euler methods is that only velocity level kinematic exressions need to be develoed in order to secify the energy functions, secifically kinetic energy. In this section, a framework is resented for solving a class of roblems in which formulation of kinetic and otential energy functions are readily formed. The main result here is that automatic differentiation tools are very well suited for directly imlementing the Lagrangian formulation in solving this imortant class of engineering roblems. Equation (4) resents the most general form of Lagrange s equations, including generalized forces and constraint forces. d L L = Q + C dt q& q T λ (4) subject to C q& = b where the Lagrangian is defined as L = T V, Q are the generalized forces, C is the constraint Jacobian matrix, and λ is the vector of Lagrange multiliers. The kinetic energy (T ) is written as a function of the generalized coordinates ( q ) and the generalized velocities ( q& ), and the otential energy (V ) is a function of the generalized coordinates. The imlied derivatives with resect to the generalized coordinates and generalized velocities in these equations can be readily comuted by automatic differentiation by simly secifying the Lagrangian function. OCEA comletely automates all of these comutations in the background. Organizing these equations in a manner in which they can be integrated; however, requires exlicitly solving for the

5 acceleration terms, which are buried in the first term of Eq. (4). The details can be found in [6]; therefore, we only summarize the key develoments here. In order to integrate the equations of motion we need to comute the right hand side of a modified form of Eq. (4), which is given in Eq. (5). L && & (5) q - T q= M Mq + +Q+ C λ In order to comute the mass matrix and its time derivative come from the first term on the left hand side of Eq. (4). Considering the most common case of a natural system, T T = T( q,q& ) = q& M( q) q& we have d L T T = q&& + j dt q& q& q& q& q i i j i j = mq&& + mq & & ij j ij j q& j (6) where T T M m = and M& m& = ij ij q& q& q& q i j i j (7) By utilizing automatic differentiation, the constraint Jacobian matrix can also be formed automatically for a holonomic constraint of the form φ ( q) = 0 since C = φ q. As has been shown above, generating the equations given in Eq. (5) can be accomlished by simly secifying the Lagrangian function, the constraint relation, and the generalized forces. OCEA accomlishes all required derivative oerations leading to the right hand side of Eq. (5). The background second artials of T, for examle, can be accessed to obtain M from Eq. (7). Formulations for flexible bodies When the rigid body assumtion is used to model a dynamical system, there is no need to consider satial integrals over the body for comuting kinetic and otential energy exressions (i.e. the Lagrangian). However, when flexibility is considered we encounter a Lagrangian exression which is comuted as an integral over the volume of the body, and, of course, we require additional coordinates to define deformations. For the case of slender beams, we can simlify this to an integral over the length of the body. Furthermore, what is often done to simlify the formulation of equations of motion for flexible dynamical systems is the introduction of aroximations for the flexible motion coordinates that aid in roducing a Lagrangian with no exlicit deendence on the satial coordinates. Examles of aroximation techniques include the well known Finite Element

6 Method and Method of Assumed Modes, [6,8]. In essence, these techniques make it ossible to roduce a Lagrangian for the flexible dynamical system of the same form as that of a rigid body dynamical system. Thus, once the satial discretization aroximations are utilized, we can roceed to generate equations of motion for a flexible dynamical system just as we do for a rigid body dynamical system by directly imlementing Lagrange s Equations of the form of Eq. (5). In this section, we demonstrate the use of OCEA in generating equations of motion for systems comrised of flexible elements. Toward this end, we develo recursive exressions for kinetic and otential energy functions for a series of linked flexible beams. Since we are not considering raid angular motions of the beams, we model the beams using Euler-Bernoulli assumtions. Generalization for multile flexible bodies We now consider generating equations of motion for a chain of linked flexible bodies. It is assumed that the first link is inned without translation, and successive links are joined with ins as shown in Figure. U to this oint, we have not discussed the choice for generalized coordinates which is an imortant matter for an automated rocess. Make note in Figure that we choose absolute angular coordinates, which are measured with resect to a common frame, in this case the horizontal. Note the x i axis connects the tis of the flexible members; thus the elastic deformation of each domain vanish at the ends of that domain. Y y x y x x θ θ y θ X Figure : Geometry of multile flexible link configuration The main develoment of this section is a recursion for the kinetic energy of the (+) th link of the form:

7 L + T = ρ ( x ) ( x, t) ( x, t) dx r& r & (8) where ρ + ( x + ) is the mass density distribution and r& + ( x, ) + t is the velocity exression for the (+) th link. We begin the develoment of the velocity exression by looking at the first beam in the chain. The osition and velocity of any oint along the first beam are written as follows: r ( x, t) = x iˆ + v ( x, t) ˆj (9) ( & θ ) r& ( x, t) = & θ v ( x, t) iˆ + v& ( x, t) + x ˆj (0) where v( x, t) is the transverse beam deformation, & θ is angular velocity, and x is the coordinate measurement along the beam frame of reference. The velocity of any oint along the second beam in the chain can be written as r& ( x, t) = r& ( L, t) & θ v ( x, t) iˆ + v& ( x, t) + x & θ ˆj () ( ) and, in general, the velocity exression for the (+) th link can be written as r& ( x, t) = r& ( L, t) & θ v ( x, t)ˆ i ( v& ( x, t) x & θ ) + + ˆj () where the velocity of the ti of the th link, r& ( L, t), is written as ( L, t) = L & θ ˆ + L & θ ˆ L & θ ˆ L & θ ˆ = i i i i= r& j j j j (3) Here, we have enforced the zero ti deformation constraint by choosing admissible functions with zero deflection at the endoints (i.e. sin( iπ x+ φ ) +, i = L ),[3]. In + this way, the beams satisfy inned-inned boundary conditions. Additionally, the exression for beam ti velocity in Eq. (3) has been greatly simlified since v ( L, t) = v & ( L, t) = 0. Equation () can be written as r& ( x, t) = L & θ ˆj & θ v ( x, t) iˆ + + i i i i= ( v& ( x, t) x & θ ) + + ˆj Now, we can rewrite the kinetic energy exression of Eq. (8) as (4)

8 L + T = ρ ( x, t) ( x, t) dx r& r & = L & ˆ ˆ θ j L & θ i i j i j j j i= j= + L & ˆ ˆ ˆ θ j & θ v v x θ i i i + + & j i= + & θ v + v& + x & θ v& + x & θ { (& ) } i (5) We can roceed further in simlifying Eq. (5) by carrying out the remaining dot roducts. If we consider absolute angular coordinates which are measured from a common reference frame, then we can write the following exressions that relate any two frames, here frames i and j. iˆ cos( θ θ ) sin( θ θ ) ˆ j i j i i i j ˆ = sin( θ θ ) cos( θ θ ) ˆ ji j i j i j j At this oint, we can introduce aroximations in order to facilitate automatic generation of ODE s in terms of the time deendent variables by the assumed modes method. In Eq. (7) we introduce an exression for v+ ( x+, t) : + + +, i +, i T (6) v ( x, t) = q ( t) φ ( x ) =q ( t) φ ( x ). (7) We note here that the first index before the comma denotes the body (+), and the indices after the comma indicate the index of the element of the array (i) in the tyical mathematical notation. Now with the relation given by Eq. (6) and the aroximation given by Eq. (7), we can write Eq. (5) as T + P+ = T ( qqθθ, &,, &) T ( ) q b sin ( ) = m L & θ L & θ cos θ θ & θ L & θ θ θ + i i j j j i i i + i i= j= i= ( ) cos( ) T b L & cos & m L L & + + i i + i i i + i i= i= + q& θ θ θ + θ θ θ θ + & θ q M q + q& M q& + & θ T T q& a + m L & θ T (8) where q, q&, θ, and θ & are the vectors containing all of the time deendent quantities for the flexible coordinates and the angular coordinates. The elements of

9 the mass matrix, M +, and the vectors, a + and b + are given in Eqs. (9-) resectively. L m + + M = ρ φ φ dx δ +, ij = (9) + +, i +, j + ij 0 L m L a = ρ x φ dx = cos( iπ) +,i , i + iπ (0) L m L + + b = ρ φ dx =, ( cos( iπ) +,i ) i + iπ () Equation (8) is used to roduce the kinetic energy for the second beam and so on for. The otential energy due to bending is given as before as L '' '' ( q ) = ( ) φ φ + + +, i +, j , i +, j + T = q K q V q q EI x dx The kinetic and otential energy of the first link are to be secified individually as given in Eqs. (3) and (4), resectively. T T T T = & θ q M q + q& M q& + & θ q a + ml & θ (3) V 6 T () = q Kq (4) With Eqs. (8) and (-4) we can form the Lagrangian function exlicitly in terms of the time deendent coordinates ( qqθθ,&,, & ) and imlement Lagrange s Equations of the standard form given in Eq. (4) in order to roduce the equations of motion. Examles In this section three examles are resented which demonstrate the use of automatic differentiation for automatic generation and integration of equations of motion. The first is a simle illustrative examle, and the final two examles demonstrate solutions for oen- and closed-chain toologies of flexible bodies. Sring endulum by forming the Lagrangian function Here, we resent an examle in which the equations of motion are automatically generated and integrated by exlicitly forming the Lagrangian function. The sring endulum, as shown in Figure, is a simle two degree of freedom examle which can be readily solved by hand. However, we resent it here in order to overview the method, which can be alied generally to solving additional roblems.

10 r θ Figure : Sring endulum As was mentioned reviously, in order to solve this tye of roblem we need to simly secify the Lagrangian function, the constraint relations (if they exist), and the generalized forces, and as well, the system hysical arameters and initial conditions. The Lagrangian function is L= T V, where T = m( r& + r & θ ) (5) and V = k( r r ) + mg( r rcos θ ) (6) 0 0 For this roblem, no constraints need be secified and no additional forces need to be accounted for. However, it would be a simle matter to include force law exressions for daming elements or drag, or to modify the sring force model to account for these effects. In addition to the integration arameters, these are the only arameters to be secified in order to comute the solution. Only Eqs. (5) and (6) need to be rogrammed by the user. A selection from the subroutine containing the above secified roblem data for the sring endulum is given in Aendix A. Here, it should be noted that the subroutine begins by including the EB_Handling routine (USE EB_HANDLING) which secifies that this subroutine contains data objects which are to be differentiated. Embedded variables and embedded functions are declared as TYPE(EB) which is an OCEA defined variable tye. The secification of the kinetic and otential energy functions is highlighted in order to show that embedded functions are tyed in a standard user- friendly manner. The structure of OCEA is such that by invoking EB_Handling, derivatives are automatically comuted in the background without user intervention. Extraction of artial derivative information is also highlighted to show the ease with which the background-comuted information can be accessed. Here, the analyst simly needs to define the dimension of the artial derivatives to be extracted. By setting this variable equal to the embedded function variable, for examle, HES_L = L, we can readily extract the Hessian of the Lagrangian in order to get access to the mass matrix and its time derivative. The overhead associated with deriving, coding, and validating the equations of motion has been avoided.

11 Oen-chain toology of flexible bodies In this section, we resent simulated results for multile flexible beams in an oenchain toology. Here, we define the kinetic and otential energy for the first link with Eqs. (3) and (4), resectively. For the second link and so on (= and so on) we define the kinetic and otential energy by Eqs. (8) and (). The system is comrised of three beams, each with mass of kg, length of 0 m, and stiffness (EI) of 4e3 Nm. The beams are initially oriented with angles { θ, θ, θ 3} = 3π 3 3 {, π, π } as shown in Figure. All initial deflections are zero with the excetion of the midoint deflection of the third beam, which is q 3, = 0.0. All initial velocities are zero with the excetion of the angular velocity of the third beam ( & θ = 0.5 rad ). sec With the ability to quickly generate models and solutions for the motion, a considerable number of analyses are readily available. Here, we show results for the rigid body and flexible contributions to the kinetic energy of the individual links as shown in Figures (3-5). Here we see that for this quite flexible system, the magnitudes of the kinetic energy due to flexibility (dotted lines) are of significant amlitude. The integration ste size was chosen to satisfy constancy of total energy. Figures 3-5: Kinetic Energy for Links, and 3

12 Closed-chain toology of flexible bodies In this section, we resent simulated results for multile flexible beams in a closedchain constrained toology. Here, we define the kinetic and otential energy for the links as we did before; however, in this case, we must simly comute the constraint forces in order to satisfy the geometric constraints, which are of the holonomic form: n L cos( ) i θ i D i = φ = = 0 (7) Li sin( θ ) in = i where n = 5 links and D= 0 m. Constraint forces are comuted by solving for the Lagrange multiliers using the Range-Sace Method. Additionally, we include otential due to gravity from rest with initial angles for the links of { θ, θ, θ3, θ4, θ 5} = 5π 7 3 {, π,0, π, π }. Initially, there is no flexible energy in the system. Daming is included at all joints with the excetion of the base joint at { XY, } = { 0,0}. The roerties of the links is identical to the revious examle with the excetion that EI = 4e4 Nm. Again, conservation of total energy was used to determine the integration ste size. Figure 6 shows a few sna shots of the motion history for the 0 second simulation. Figure 6: Motion for a 5 flexible link closed-chain system

13 Conclusion In this aer we resent a new method for the automatic generation and integration of equations of motion using oerator overloading techniques. The automatic differentiation tool (OCEA: Object Oriented Coordinate Embedding Method) was utilized to comute the artial derivatives required for imlementing the traditional Lagrangian equations of motion. Several examles were resented to demonstrate the utility of automatic differentiation in generating equations of motion. A generalized simulation code was resented which can comute the motion for systems containing many flexible bodies. This generalized code was shown to solve examle for an oen chain, unconstrained toology and a closed chain constrained toology. Nonlinear multile flexible body roblems having an energy integral were considered. We used the constancy of the energy constant as evidence that accurate solutions were obtained. References [] Banerjee, A. K., Contributions of Multibody Dynamics to Sace Flight: A Brief Review, Journal of Guidance, Control, and Dynamics, Vol. 6, No. 3, , May-June 003. [] Baruh, H., Analytical Dynamics, McGraw Hill, New York, 998. [3] Baruh,H. and Radisavljevic, V., Modeling of Flexible Mechanisms by Constrained Coordinates, Journal of the Chinese Society of Mechanical Engineers, Vol.,. -4, No., 000. [4] Durrbaum, A., Klier, W., and Hahn, H., Comarison of Automatic and Symbolic Differentiation in Mathematical Modeling and Comuter Simulation of Rigid Body Systems, Multibody System Dynamics, Vol. 7, , 00. [5] Griffith, D.T., Sinclair, A.J., Turner, J.D., Hurtado, J.E., and Junkins, J.L., Automatic Generation and Integration of Equations of Motion by Oerator- Overloading Techniques, AAS/AIAA Saceflight Mechanics Meeting, Maui, HI, USA, Paer AAS 04-4, February 8-, 004. [6] Junkins, J.L. and Kim, Y., Introduction to Dynamics and Control of Flexible Structures, AIAA Education Series, 993. [7] Schaub, H., and Junkins, J.L., Analytical Mechanics of Sace Systems, AIAA, 003. [8] Thomson, W.T., and Dahleh, M.D., Theory of Vibration with Alications, Prentice Hall, New Jersey, 998. [9] Turner, J. D., Automated Generation Of High-Order Partial Derivative Models, AIAA Journal, August 003. [0] Turner, J. D., Generalized Gradient Search And Newton's Methods For Multilinear Algebra Root-Solving And Otimization Alications, Invited Paer No. AAS 03-6, to Aear In The Proceedings Of The John L. Junkins Astrodynamics Symosium, George Bush Conference Center, College Station, Texas, May 3-4, 003.

14 APPENDIX A: FORTRAN 90 Subroutine for Sring Pendulum Note: X0() = r; X0() = θ ; X0(3) = r& ; X0(4) = & θ SUBROUTINE SPRING_PEND_EQNS( PASS, TIME, X0, DXDT, FLAG ) USE EB_HANDLING IMPLICIT NONE!...ARGUMENT LIST VARIABLES ********** REAL(DP), INTENT(IN):: TIME TYPE(EB),DIMENSION(NV),INTENT(IN ):: X0 TYPE(EB),DIMENSION(NV), INTENT(INOUT):: DXDT!...LOCAL VARIABLES TYPE(EB)::L, T, V! LAGRANGIAN, KINETIC, POTENTIAL REAL(DP):: M, K! MASS AND STIFFNESS VALUES REAL(DP), DIMENSION(NV):: JAC_L REAL(DP), DIMENSION(NV,NV):: HES_L REAL(DP), DIMENSION(NV/,NV/):: MASS, MASS_INVERSE, MASSDOT REAL(DP), DIMENSION(NV/):: JAC_L_Q, QDOTDOT, QDOT TYPE(EB):: R0! UNSTRETCHED SPRING LENGTH REAL(DP):: GRAV M =.0D0; K = 75.0D0! MASS AND STIFFNESS *********** T = 0.5D0*M*(X0(3)** + X0()***X0(4)**)! DEFINE KE V = 0.5D0*K*(X0()-R0)** + M*GRAV*(R0-X0()*COS(X0())) L = T V! DEFINE LAGRANGIAN FUNCTION! PE JAC_L = L! EXTRACT JACOBIAN OF LAGRANGIAN JAC_L_Q = JAC_L(:NV/)! EXTRACT PARTIALS W.R.T. GEN. COORDS. HES_L = L! EXTRACT nd ORDER PARTIALS OF LAGRANGIAN MASS = HES_L(NV/+:NV,NV/+:NV)! EXTRACT MASS MATRIX MASSDOT = HES_L(NV/+:NV,:NV/)! EXTRACT MDOT ********** QDOTDOT = MATMUL(MASS_INVERSE,(JAC_L_Q &- (MATMUL(MASSDOT,QDOT)))) DXDT()%E = X0(3)%E DXDT()%E = X0(4)%E DXDT(3)%E = QDOTDOT() DXDT(4)%E = QDOTDOT()! RDOT! THETADOT! RDOTDOT! THETADOTDOT END SUBROUTINE SPRING_PEND_EQNS