Teaching Multibody System Simulation: An Approach with MATLAB

AC 2012-3164: TEACHING MULTIBODY SYSTEM SIMULATION: AN APPROACH WITH MATLAB Dr. Peter Wolfsteiner, Munich University of Applied Sciences Peter Wolfsteiner is professor in mechanical engineering at the Munich University of Applied Sciences (HM) in Germany. He received his Ph.D. degree in M.E. from the Technical University Munich. Prior to joining the faculty at HM, he worked at Knorr-Bremse Group as a Manager in the area of new technologies for rail vehicle braking systems. He teaches undergraduate and graduate courses in statics, strength of materials, dynamics, controls, numerics, and simulation of dynamical systems. Research interests include simulation, nonlinear dynamics, random vibrations, and fatigue. He is currently working as exchange professor at California Polytechnic State University in San Luis Obispo.

c

American Society for Engineering Education, 2012

Teaching Multibody System Simulation, an Approach with MATLAB Abstract Teaching Multibody Systems needs to cover the related theoretical concepts of advanced dynamics, the application of the necessary numerical methods in a sufficient depth, and needs to give students the opportunity to model and solve authentic problems on their own. The last step may only be done with the help of a computer. A variety of highly sophisticated computer programs are available (e.g., Adams), able to solve special problems from the area of Multibody Systems or even nearly every general type of engineering dynamics problem. The use of such software has some drawbacks for students however. There is a high risk to waste valuable time by learning how to use software that was not designed for educational use but was instead designed for large scale problems in industry. And, what is even worse, students do not really see how the software works, because the transition from the theoretical to numerical concepts is usually not visible in the software. To deal with this challenge, this paper presents a strategy based on the software Matlab, usually known by engineering students. The proposed method is based on doing symbolic manipulations for the derivations of the equations and on using numerical methods for the solution. The main idea is not to waste time and effort for getting the equations in a certain form needed for a computer solution, but to offer a procedure allowing students to easily use the equations as they are -known from the theoretical concept- and directly use them in Matlab. Furthermore the intention is not to be limited to simple 2D problems with just a few degrees of freedom but to offer the full range of a 3D Multibody System simulation with modelling, derivation of equations of motion, numerical solution of differential equations, calculation of static equilibrium as well as linearization and calculation of eigenvalues and eigenmodes. Based on a couple of small examples the paper gives a detailed introduction to the proposed method including all necessary equations and also the related Matlab code.

1

Introduction

The author of this paper is convinced that an up-to-date teaching of Multibody Systems is not sufficient without an application of the theoretical framework to real-life problems. Only by applying the theory by creating numerical models students are able to gain appreciation of the potentials and limitations of this theoretical concept. This paper describes a tightly integrated and easy-to-use combination of mathematical description and numerical solution without using specialized Multibody simulation software. Based on the software Matlab, the derivation of equations and their numerical solution are computed. Symbolic operations replace the fault-prone, tedious manual derivation of equations; however the mathematical steps are still visible and have to be done by the student. The numerical solutions demonstrate real effects and enable the student to get feedback on the mechanical modelling. The Matlab code integrated in the following sections is intended to show the easy transition from the equations to the programming code and enables the reader to apply the concept on his own. In this paper the concept is shown with Matlab because the functional range

(operations with vectors/matrices, symbolic and numerical tools, plentiful numerical routines) of this software meets the needs very well. Similar software tools (e.g.: MAPLE, MATHEMATICA) may also be applied. The approach to the Multibody subject in literature is manifold and varies in structure and notation. The concept used here is derived from1,2,3 and may easily be adapted to alternative concepts. Also the profitable use of numerical tools like Matlab in engineering education is well known and many different proposals have been widely discussed in a high number of publications for lots of different fields of engineering activity. However, useful combinations of symbolical and numerical tools for engineering applications seem not yet to be so much accepted. The idea of combining both in Matlab for dynamics is taken from4 , the special focus on Multibody Systems shown here seems to be not yet available in engineering education literature. This paper does not intend to explain Multibody system theory nor the basic application of Matlab. A basic knowledge in these fields is assumed to be known. The paper also does not explain the details of the mathematical derivations nor the Matlab code. It is assumed that the examples are self-explanatory if they are worked through. The paper keeps its focus on demonstrating the educational concept with some selected ’academic’ examples. They are chosen with the idea to be as simple as possible, but also able to show the capability of the proposed method. They are part of a complete Multibody Systems teaching concept for graduate students consisting of lectures with a script provided by the lecturer, homework problems giving an opportunity to practice theoretical skills and lab assignments based on the use of Matlab as shown in this paper (the knowledge of the Matlab programming language is a prerequisite for this class). Therefore students usually develop code on their own based on code examples demonstrating the basic concepts. These code examples also include a selection of library functions providing, for example, an animation function that can be used to demonstrate the results with a small movie. Figure 1 shows some (animation-) examples that have been covered within this class. 1

0.5

4 3.5

0

Y−Achse

3

Y−Achse

2.5 2

−0.5

−1 0.3 Z−Achse

1.5

0.2

0.5 0.4

0.1

−1.5

0.3

1

0 0.2 0.1

−0.4

0.5

0

−0.2

−2

−0.1

0 −0.2 0.2

0

−0.3 0.4 −0.4

1 X−Achse

2

3

−0.5

Y−Achse

0.6

0

−0.5

0 X−Achse

0.5

0.8 X−Achse

Vehicle Suspension

Tricycle Dynamics

Reciprocating Engine

Figure 1: Examples of Modelled Multibody Systems The approach has been developed by the author over the last five years and was successfully applied to graduate students at a German university of applied sciences within a masters program focused on Mechatronics. This program is job-oriented and tries to teach professional skills. The Mechatronics concentration requires a strong emphasis on dynamics,

modelling, controls, simulations and the application of related software. The use of Matlab for Multibody Systems tries to serve this need. The student feedback on this class usually shows the following three major points: very demanding: starting with pure dynamics theory and ending with programming numerical algorithms a wide range of skills is required, highly satisfying: students get very satisfied by obtaining the ability to solve real life nonlinear dynamics problems on their own by applying elementary dynamics theory without the help of special dynamics software tools, good integration in Mechatronics: the ability to model and analyze dynamics problems in Matlab fits very well into a framework of a Mechatronics Masters program which also includes controls and signal analysis in Matlab.

2

Model of an elastic beam

Figure 2: Model of an Elastic Beam with 3 dofs and Kinematic Excitation

The model of a 2D elastic beam (Figure 2) consists of three rigid elements (1), (2) and (3) each with the mass m linked together with rotational joints in Pi and rotational spring and damper elements (stiffness c, damping d). The left end of the beam model is excited kinematically in the vertical direction with a given function u(t) = uo sin(ωt). The rotations of the elements are described by three angles ϕi , the length of each element is l, each with the center of gravity Si . A Newtonian frame I is located at 0 and each beam element has a body fixed frame Ki . This model does not represent the real physics of an elastic beam, but it may be used as a simple approximation. An increase of the number of elements

improves the quality of approximation to the real behavior. However in this simple form the model is sufficient to demonstrate a couple of typical tasks relevant to the operation with Multibody problems. The following sections explain the derivation of the equations of motion. Because of its compact theoretical formulation section 2.1 starts with the Lagrange equation of second kind, section 2.2 demonstrates the more relevant procedure with the Newton-Euler equations.

2.1

Lagrange equation of second kind

For the derivation of equations of motion this section uses the Lagrange equation of second kind with the vector of minimal coordinates q = (ϕ1 , ϕ2 , ϕ3 )T , the kinetic energy T , the potential energy V and the vector of generalized forces u: d dt

∂T ∂ q˙

!

−

∂T ∂V + = uT . ∂q ∂q

(1)

The kinetic energy T may be derived for one beam element i with respect to the center of gravity Si with Equation 2: 1 1 (Si) Ti = m v TSi v Si + ω Ti I i ω i , 2 2

(2)

(S)

with the mass mi , the inertia tensor I i , the velocity v Si and the angular velocity ω i . The potential energy of the weight forces (index i) and the spring moments (index j) with Equation 3: c Vi = −mi g T r OSi ; Vj = ϕ2rel,j . (3) 2 The nonpotential moments (index j) on body i caused by the dampers are presented as Equation 4: !T ∂ϕi mj = d ϕ˙ rel,j ; uij = mj . (4) ∂q These equations allow the derivation of the equations of motion in the following form by performing the derivatives of Equation 1: ¨ − h(t, q, q) ˙ = 0. M (t, q) q

(5)

The following steps describe the implementation in Matlab. Symbolic Derivation of Equations of Motion: Listing 1 shows the first section of the Matlab code used for deriving the equations of motion with the above set of equations. Table 1 explains the code. Numerical Evaluation of Symbolic Expressions: The numerical analysis of the equations of motion requires a numerical evaluation of the derived symbolic expressions for M and h. For this purpose Matlab offers the command subs that performs a substitution of symbolic expressions with numerical ones. But the application of the subs command within

Table 1: Description of Listing 1 line:

code:





0 0 0 (S)   1 to 5 setting of parameters, the inertia tensor is: I i =  0 0 0  m 2 l 0 0 12 7 to 9 definition of symbolic variables necessary for the following derivations (phi1 → ϕ1 , phi1_p → ϕ˙ 1 , ...) 11 to 28 derivation of kinematics: • the notation of the vector K1 r P1 P2 (in Matlab: K1_r_P1P2) denotes the vector r pointing from P1 to P2 noted in the frame K1 , • the function A_z(phi) isa separate functioncalculating the transformacos ϕ − sin ϕ 0   tion matrix AIK (ϕ) =  sin ϕ cos ϕ 0  for a rotation of K with 0 0 1 respect to the frame I about the axis Z with the angle ϕ, • the velocities I v Si of the centers of gravity are calculated with the time derivatives of the position vectors I v Si = I r˙ Si , since t is not expressed explicitly in I r OSi (t, q), the time derivative is calculated with r q˙ + ∂ r , r˙ = dtd r(t, q) = ∂∂q ∂t 30 35 39 47

to to to to

33 37 45 51

kinetic energy corresponding to Equation 2 potential energy corresponding to Equation 3 generalized forces corresponding to Equation 4 derivation of equations of motion (Equation 5) corresponding to Equation 1: • since t is not expressed explicitly in T and V , the time derivatives are calculated symbolically with: ∂ ∂t



∂T ˙ ∂q

T

d dt





∂T ˙ ∂q

∂T ˙ ∂q

T

=

∂ ˙ ∂q



∂T ˙ ∂q

T

q¨ +

∂ ∂q



∂T ˙ ∂q

T

q˙ +

,

• M (t, q) =

∂ ˙ ∂q



∂T ˙ ∂q

˙ = − ∂∂q • h(t, q, q)

T



∂T ˙ ∂q

, T

q˙ −

∂ ∂t

T

+



∂T ∂q

T

−



∂V ∂q

T

+ u,

the framework of numerical integration shows that the computational effort is even for small equations very high, with the consequence of long simulation times. It seems that the call of subs launches the symbolic toolbox every time subs is called, doing lots of inefficient computations! To solve this problem, the author uses the self developed routine sym_2_fun that generates a callable Matlab-function from a symbolic expression (see Listing 2). In

Listing 1: Symbolic Derivation of Equations of Motion with Lagrange Equation of Second Kind 1 2 3 4 5

% p a r a m eter s : g = [0; −9.81;0]; l = 1; omega 0 = 5∗2∗ p i ; K I S = [0 0 0; 0 0

m c rot u 0 0; 0 0

= 1; = 100; d rot = 3; = .1; m∗ l ˆ 2 / 1 2 ] ;

6 7 8 9

% definitions : syms p h i 1 phi2 phi3 phi1 d phi2 d phi3 d t q = [ phi1 ; phi2 ; phi3 ] ; q d = [ phi1 d ; phi2 d ; phi3 d ] ; x = [ q ; q d ] ;

10 11 12 13 14

% kinematics : K1 r P1P2 = [ l ; 0 ; 0 ] ; K2 r P2P3 = [ l ; 0;0]; K1 r P1S1 = [ l / 2 ; 0 ; 0 ] ; K2 r P2S2 = [ l / 2 ; 0 ; 0 ] ; K3 r P3S3 = [ l / 2 ; 0 ; 0 ] ; A IK1 = A z ( p h i 1 ) ; A IK2 = A z ( p h i 2 ) ; A IK3 = A z ( p h i 3 ) ;

15 16 17 18 19 20 21

I I I I I I

r r r r r r

0P1 0P2 0P3 0S1 0S2 0S3

= = = = = =

[0; I r I r I r I r I r

u 0 ∗ s i n ( omega 0 ∗ t ) ; 0 ] ; 0 P 1 + A IK1 ∗ K1 r P1P2 ; 0 P 2 + A IK2 ∗ K2 r P2P3 ; 0 P 1 + A IK1 ∗ K1 r P1S1 ; 0 P 2 + A IK2 ∗ K2 r P2S2 ; 0 P 3 + A IK3 ∗ K3 r P3S3 ;

22 23 24 25 26 27 28

I v S1 = I v S2 = I v S3 = K1 omega K2 omega K3 omega

j a c o b i a n ( I r 0 S 1 , q )∗ q d + d i f f ( I r 0 S 1 , t ) ; j a c o b i a n ( I r 0 S 2 , q )∗ q d + d i f f ( I r 0 S 2 , t ) ; j a c o b i a n ( I r 0 S 3 , q )∗ q d + d i f f ( I r 0 S 3 , t ) ; IK1 = [ 0 ; 0 ; p h i 1 d ] ; IK2 = [ 0 ; 0 ; p h i 2 d ] ; IK3 = [ 0 ; 0 ; p h i 3 d ] ;

29 30 31 32 33

% kinetic T =(m∗ I v m∗ I v m∗ I v

en er g y : S 1 . ’ ∗ I v S 1 + K1 omega IK1 . ’ ∗ K I S ∗ K1 omega IK1 + . . . S 2 . ’ ∗ I v S 2 + K2 omega IK2 . ’ ∗ K I S ∗ K2 omega IK2 + . . . S 3 . ’ ∗ I v S 3 + K3 omega IK3 . ’ ∗ K I S ∗ K3 omega IK3 ) / 2 ;

34 35 36 37

% p o t e n t i a l en er g y : V = c r o t / 2 ∗ ( p h i 1 ˆ2 + ( phi2−p h i 1 )ˆ 2 + ( phi3−p h i 2 ) ˆ 2 ) − . . . m∗ g . ’ ∗ ( I r 0 S 1 + I r 0 S 2 + I r 0 S 3 ) ;

38 39 40 41 42 43 44 45

% non−p o t e n t i a l f o r c e s ( r o t . dampers ) : m p1 = ( p h i 1 d )∗ d r o t ; m p2 = ( p h i 2 d −p h i 1 d )∗ d r o t ; m p3 = ( p h i 3 d −p h i 2 d )∗ d r o t ; u = j a c o b i a n ( phi1 , q ) . ’ ∗ ( m p2−m p1 ) + . . . j a c o b i a n ( phi2 , q ) . ’ ∗ ( m p3−m p2 ) + . . . j a c o b i a n ( phi3 , q ) . ’ ∗ ( −m p3 ) ;

46 47 48 49 50 51

% e q u a t i o n s o f motion : M = s i m p l i f y ( j a c o b i a n ( j a c o b i a n (T, q d ) . ’ , q d ) ) ; h = s i m p l i f y(− j a c o b i a n ( j a c o b i a n (T, q d ) . ’ , q )∗ q d . . . −j a c o b i a n ( j a c o b i a n (T, q d ) . ’ , t ) . . . +j a c o b i a n (T, q ) . ’ −j a c o b i a n (V, q ) . ’ + u ) ;

a first step the routine coo_2_vec replaces the elements of the state vector x (phi1, phi2, phi3, phi_1, phi_2, phi_3) by the array x(1:6), this makes the application of the generated routine easier. In the second step a Matlab-function is generated with the input variables x and t. The name of the generated function is stored in the variables f.M and f.h. Listing 2 ˙ (lines 7 and 8). The functions generated for shows the procedure for M (t, q) and h(t, q, q) the positions and rotations (lines 1 to 6) are needed later for the animation of the results.

The functions in line 9 and 10 are needed for the calculation of the static equilibrium and the numerical integration. The transition from symbolic to numeric calculations is done here ˙ although the expression M −1 h is needed within the numerical with M (t, q) and h(t, q, q), integration (see Numerical Integration). This is because the symbolic inversion of M is an extensive operation and produces a very large symbolic expression. Listing 2: Derivation of Function Handles 1 2 3 4 5 6 7 8 9 10

f f f f f f f f f f

. r o t {1} . r o t {2} . r o t {3} . pos {1} . pos {2} . pos {3} .h .M . ode fcn . equ fcn

= = = = = = = = = =

sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo @ o d e f cn ; @elastic beam

2 2 2 2 2 2 2 2

v e c ( A IK1 , x ) , v e c ( A IK2 , x ) , v e c ( A IK3 , x ) , vec ( I r 0P1 , x ) , vec ( I r 0P2 , x ) , vec ( I r 0P3 , x ) , vec (h , x ) , v e c (M, x ) ,

’x , t ’x , t ’x , t ’x, t ’x, t ’x, t ’x , t ’x , t

’); ’); ’); ’); ’); ’); ’); ’);

equilibrium fcn ;

Animation: To make the interpretation of the results easier, the author uses a self developed animation routine (animation), capable of demonstrating the motion of predefined 3d-wireframe models on the screen within a Matlab figure. The definition of the bodies used in the current example is shown in Listing 3. The animation is applied later for the static equilibrium, the eigenmodes and the numerical integration (see Listing 4, 6, 7). The routine and its interface will not be explained in detail here. Listing 3: Definitions for the Animation 1 2 3 4

body body { 1 } . geometry body { 2 } . geometry body { 3 } . geometry

= = = =

wireframe rectangle xy ( l , l /10); w i r e f r a m e o f f s e t ( body , [ l / 2 ; 0 ; 0 ] ) ; body { 1 } . c o l o r =1; body { 1 } . geometry ; body { 2 } . c o l o r =2; body { 1 } . geometry ; body { 3 } . c o l o r =3;

Static Equilibrium: Listings 4 and 5 show the code used for calculating the static equilibrium. The equation h(t = 0, q o , q˙ = 0) = 0 (6) (see Listing 5, Line 2) defines a nonlinear set of equations, whose solution vector contains the angles q o = (ϕ1o , ϕ2o , ϕ3o )T of the static equilibrium. The Matlab function fsolve is used to find a numerical solution of the nonlinear equations. The function animation shows the result on the screen (see Figure 3, left). Listing 4: Numerical Calculation of Static Equilibrium 1 2 3

q start = [0 0 0]; q 0 = f s o l v e ( f . equ fcn , q s tar t , [ ] , f ) ; a n i m a ti o n ( 0 , q 0 , f , body , 0 , [ 0 , 9 0 ] ) ;

Listing 5: Function for the Calculation of the Static Equilibrium 1 2 3

f u n c t i o n n u l l=e l a s t i c b e a m e q u i l i b r i u m f c n ( q , f ) null = f .h ([ q 0 0 0] ,0); end

Linear Analysis: In general, Multibody Systems are nonlinear and usually call for a corresponding (numerical) solution method. A linear analysis is reasonable, if the deviation from the linearization point is within a certain limit. If this approximation is possible, the results of a linear analysis offer a much better comprehension of the mechanical character of system than single numerical solutions. The code in Listing 6 performs the linearization of the equations of motion about the static equilibrium and solves the eigenproblem numerically with the Matlab routine eig. Table 2 explains the code. Listing 6: Symbolic Linearization and Numerical Calculation of Eigenvalues and -modes 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16

% linearization : q R = sym ( q 0 ) . ’ ; q d R = [ sym ( 0 ) ; sym ( 0 ) ; sym ( 0 ) ] ; q dd R = q d R ; t R=sym ( 0 ) ; x R = [q R;q d R ]; M l i n = d o u b l e ( s u b s (M, x , x R , 0 ) ) ; B l i n = d o u b l e (− s u b s ( j a c o b i a n ( h , q d ) , x , x R , 0 ) ) ; C l i n = d o u b l e ( s u b s ( j a c o b i a n ( (M∗ q dd R ) , q ) , x , x R , 0 ) . . . −s u b s ( j a c o b i a n ( h , q ) , [ x ; t ] , [ x R ; t R ] , 0 ) ) ; % e i g e n f r e q u e n c i e s o f damped system : lambda = e i g ( [ 0∗ z e r o s ( s i z e ( M l i n ) ) ey e ( s i z e ( M l i n ) ) ; . . . −i n v ( M l i n )∗ C l i n −i n v ( M l i n )∗ B l i n ] ) ; disp ( [ ’ eigenf r equencies : ’ ... num2str ( s o r t ( imag ( lambda ( [ 1 3 5 ] ) ) ’ / 2 / p i ) ) ] ) ; % ei g en m o d es o f undamped system : [ modes , omega]= e i g ( C l i n , M l i n ) ; a n i m a ti o n ( [ 0 0 0 ] , . 3 ∗ modes ’ , f , body , − 1 , [ 0 , 9 0 ] ) ;

Figure 3: Left: Statical Bending of Elastic Beam, Right: Three Eigenmodes of Elastic Beam

line: 1 to 4

5 to 8

Table 2: Description of Listing 6 code: definition of reference motion (index R): the position is the static equilibrium ¨ R = 0, tR = 0), (q R = q o ), velocity, acceleration and time are set to zero (q˙ R = q for the following subs command these variables have to be defined symbolically! ˙ q¨ = q¨ R + y ¨) the deviations from the reference motion (q = q R + y, q˙ = q˙ R + y, are linearized with a taylor series, the resulting linear differential equation is: ¨ + B(t) y˙ + C(t) y = 0: M (t) y



¨ ) ∂(M (q, t) q ∂(M (q, t) q¨ ) ¨ (t) • M (q, t) q¨ ≈ M (q R , t) q¨ R + y(t) + y ∂q ∂¨ q R R



˙ t) ˙ t) ∂h(q, q, ∂h(q, q, ˙ t) ≈ h(q R , q˙ R , t) + ˙ y(t) + • h(q, q, y(t) ∂q ∂ q˙ R R

• M (t) = M (q R , t) • B(t) = −



˙ t) ∂h(q, q, ∂ q˙ R



˙ t) ¨ R ∂h(q, q, ∂M (q, t) q − • C(t) = ∂q ∂q R R • with:

. . .

=

R

. . .

q=qR ˙ q˙ R q= ¨ =¨ q qR

9 to 13 calculation of eigenfrequencies of damped system by solving the eigenproblem ˙ T: of the state transformed system with x = (y, y) E −M −1 B

0

• differential equation: x˙ =

−M −1 C |

{z

A

!

x

}

• equation for eigenvalues λ: (A − λE) = 0 (with conjugate-complex λ) ˆ : λˆ • equation for eigenvectors x x = Aˆ x 14 to 16 calculation of eigenmodes with original differential equation without damping and displaying the modes with animation (see Figure 3, right): • equation for eigenvalues ω: (C − ω 2 M ) = 0 ˆ : ω2M q ˆ = Cq ˆ • equation for eigenvectors x

Numerical Integration: The time solution of the nonlinear differential equations is performed numerically with one of the ode routines of Matlab; they require a function with the differential equations transformed to state space (see function ode_fcn, Listing 8). The calculated results are plotted with the Matlab plot command (representative results see Figure 4), the motion is shown on the screen with animation routine already described (see Listing 7). Table 3 explains the code. Listing 7: Numerical Integration with Plots and Animation 1 2 3 4 5 6 7 8 9 10 11 12 13 14

% numerical i n t e g r a t i o n : x 0 = [ 1 0 0 −10 0 0 0 ] ∗ p i / 1 8 0 ; t end = 10; f i g u r e ; opt = o d e s e t ( ’ OutputFcn ’ , @odeplot ) ; [ t num , x num ] = ode23 ( f . o d e f c n , [ 0 t e n d ] , x 0 , opt , f ) ; % plot results : s u b p l o t ( 1 , 2 , 1 ) ; p l o t ( t num , x num ( : , 1 : 3 ) ∗ 1 8 0 / 2 / p i ) ; x l a b e l ( ’ t i n s ’ ) ; g r i d on ; l e g e n d ( ’ 1 ’ , ’ 2 ’ , ’ 3 ’ ) ; y l a b e l ( ’ a n g l e i n d eg r ee ’ ) ; s u b p l o t ( 1 , 2 , 2 ) ; p l o t ( t num , x num ( : , 4 : 6 ) ∗ 1 8 0 / 2 / p i ) ; x l a b e l ( ’ t i n s ’ ) ; g r i d on ; l e g e n d ( ’ 1 ’ , ’ 2 ’ , ’ 3 ’ ) ; y l a b e l ( ’ a n g u l a r v e l . i n ˆ o / s ’ ) ; % a n i m a ti o n : dt = 0.01; t i n t e r p =0: dt : t num ( end ) ; x i n t e r p=i n t e r p 1 ( t num , x num , t i n t e r p ) ; a n i m a ti o n ( t i n t e r p , x i n t e r p , f , body , dt , [ 0 , 9 0 ] ) ;

Listing 8: Function for Ode-Solver 1 2 3 4

f u n c t i o n dxdt=o d e f c n ( t , x , f ) h = f . h ( x , t ) ; M = f .M( x , t ) ; dxdt = [ x ( ( end / 2 )+ 1 : end ) ; M\h ] ; end

line: 1 to 5 6 to 10 11 to 14 2 3

Table 3: Numerical Integration description of Listing 7: numerical integration with ode23 plot of results animation with equidistant time steps description of Listing 8: ˙ T calculation of h(t, x) and M (t, x) with x = (q,!q) q˙ ˙ x) = calculation of x(t, −1 ˙ M (t, q) h(t, q, q)

Complete Code: Now the parts of the code may be put together in one Matlab function doing the symbolic derivation (Listing 1), the generation of functions (Listing 2), the definition of animation parameters (Listing 3), the calculation of the static equilibrium (Listing 4), the linear analysis (Listing 6) and the numerical integration (Listing 7). For this example the numerical integration in Matlab on a modern PC works without using significant

Figure 4: Typical Simulation Results CPU time. With stiff systems or a growing number of degrees of freedom the computational time grows fast to an impractical limit. This is mainly caused by the inefficient operation mode of Matlab. However, the performance is sufficient for examples with an educational background. Practical problems with a high number of dofs need appropriate software. The derivation with the Lagrange equation of second kind is also a very limited method for deriving the equations of motion, because the computation time for calculating symbolically the derivatives of T and V (Equation 1) may grow extremely with an increasing number of dofs in combination with a complex kinematic structure. Therefore usually the Newton-Euler method is used, since it does not have this limitation. The equations of motion may be derived separately for every body. The next section shows this procedure.

2.2

Newton-Euler Equation n h X

i

J TT i (p˙i − f ei ) + J TRi (l˙i − mei ) = 0

i=1

(7)

The basic Equation 7 for an Multibody system is based on the Jacobian matrices for translational and rotational motion (J T i and J Ri ), and the principles of linear (p˙i − f ei = 0) and angular (l˙i − mei = 0) momentum (with linear and angular momentum pi and li , applied forces and moments f ei and mei ) of all n bodies. Noting the translational parts in frame I and (Si) the rotational ones in the corresponding body reference frame Ki (with Ki I i noted in the corresponding reference frame Ki ) and choosing the centers of gravity Si as reference points, Equation 7 may be rewritten to Equation 9 with the relations corresponding to Equation 8 ˜ (the cross product is replaced by a matrix operation: ω × r = ωr). ˙i Ip n X

˙ (Si ) Ki l i

= mi I r¨ Si ; T IJTi

(Si )

= Ki I i

˙ IKi Ki ω

(Si )

˜ IKi Ki I i + Ki ω

Ki ω IKi

(8)

[mi I r¨ Si − I f ei ] +

i=1

T Ki J Ri

h

(Si)

Ki I i

˙ IKi + Ki ω ˜ IKi Ki ω

(Si )

Ki I i

e(Si )

Ki ω IKi − Ki mi

i

=0

(9)

Together with the kinematics of the bodies expressed by generalized coordinates q (Equation 10), Equation 9 may be rearranged to Equation 11 and then to Equation 12 with an ˙ expression for M (t, q) and h(t, q, q). ¨ Si = I J T i q¨ + I ¯ιT i ; ˙ IKi = Ki J Ri q¨ + Ki ¯ιRi Ir Ki ω n n X T ¨ + I ¯ιT i ) − I f ei ] + I J T i [mi (I J T i q i h (Si ) (S ) T ˜ IKi Ki I i i Ki ω IKi (Ki J Ri q¨ + Ki ¯ιRi ) + Ki ω Ki J Ri Ki I i n n X

mi I J TT i I J T i + Ki J TRi

(Si ) Ki I i Ki J Ri

i

|

{z

(11) e(Si )

− K i mi

io

=0

q¨ −

(12)

}

M (q, t)

n n X

o

(10)

−I J TT i [mi I ¯ιT i − I f ei ] −

i

|

h

(Si ) T ιRi Ki ¯ Ki J Ri Ki I i

˜ IKi + Ki ω {z

(Si ) Ki I i Ki ω IKi

˙ t) h(q, q,

e(Si )

− K i mi

io

=0

}

With these basics the equations of motion of the example of Figure 2 may be derived again. The applied forces are given by the weight of the beam elements and the applied moments by the rotational springs and dampers. Symbolic Derivation of Equations of Motion: Listing 9 shows the section of the Matlab code used for deriving the equations of motion with the above set of equations. This code replaces the derivation with the Lagrange equation of second kind, Listing 1. The results are identical. As explained previously, the computational time for this symbolic derivation grows slower with the number of dofs compared to the Lagrange equation of second kind. Table 4 explains the code. This code may be written more compact e.g. by introducing a subfunction for deriving M and h for every single body. This was not done here, to keep the code easily readable and as close as possible to the original equations.

2.3

Elastic Beam with Contact

Figure 5: Model of the Elastic Beam with a Unilateral Spring Contact in P4

Listing 9: Symbolic Derivation of Equations of Motion with Newton-Euler Equation 1 2 3 4 5

% p a r a m eter s : g = [0; −9.81;0]; l = 1; omega 0 = 5∗2∗ p i ; K I S = [0 0 0; 0 0

m c rot u 0 0; 0 0

= 1; = 100; d rot = 3; = .1; m∗ l ˆ 2 / 1 2 ] ;

6 7 8 9

% definitions : syms p h i 1 phi2 phi3 phi1 d phi2 d phi3 d t q = [ phi1 ; phi2 ; phi3 ] ; q d = [ phi1 d ; phi2 d ; phi3 d ] ; x = [ q ; q d ] ;

10 11 12 13 14 15 16 17

% k i n e m a t i c s body 1 : I r 0P1 = [ 0 ; u 0 ∗ s i n ( omega 0 ∗ t ) ; 0 ] ; A IK1 = A z ( p h i 1 ) ; K1 r P1S1 = [ l / 2 ; 0 ; 0 ] ; I r 0 S 1 = I r 0 P 1 + A IK1 ∗ K1 r P1S1 ; I J S1 = jacobian ( I r 0S1 , q ) ; I v S1 = I J S1 ∗q d + d i f f ( I r 0S1 , t ) ; I j bar S1 = j a c o b i a n ( I v S 1 , q )∗ q d + d i f f ( I v S 1 , t ) ; K1 omega IK1 = [ 0 ; 0 ; p h i 1 d ] ; K J R1 = j a c o b i a n ( K1 omega IK1 , q d ) ; K j bar R1 = j a c o b i a n ( K1 omega IK1 , q )∗ q d + d i f f ( K1 omega IK1 , t ) ;

18 19 20 21 22 23 24 25 26

% k i n e m a t i c s body 2 : K1 r P1P2 = [ l ; 0 ; 0 ] ; I r 0 P 2 = I r 0 P 1+A IK1 ∗ K1 r P1P2 ; K2 r P2S2 = [ l / 2 ; 0 ; 0 ] ; A IK2 = A z ( phi2 ) ; I r 0S2 = I r 0 P 2 + A IK2 ∗ K2 r P2S2 ; I J S 2 = j a c o b i a n ( I r 0 S 2 , q ) ; I v S 2=I J S 2 ∗ q d + d i f f ( I r 0 S 2 , t ) ; I j bar S2 = j a c o b i a n ( I v S 2 , q )∗ q d + d i f f ( I v S 2 , t ) ; K2 omega IK2 = [ 0 ; 0 ; p h i 2 d ] ; K J R2 = j a c o b i a n ( K2 omega IK2 , q d ) ; K j bar R2 = j a c o b i a n ( K2 omega IK2 , q )∗ q d + d i f f ( K2 omega IK2 , t ) ;

27 28 29 30 31 32 33 34 35

% k i n e m a t i c s body 3 : K2 r P2P3 = [ l ; 0 ; 0 ] ; I r 0 P 3 = I r 0 P 2 + A IK2 ∗ K2 r P2P3 ; K3 r P3S3 = [ l / 2 ; 0 ; 0 ] ; A IK3 = A z ( phi3 ) ; I r 0S3 = I r 0 P 3 + A IK3 ∗ K3 r P3S3 ; I J S 3 = j a c o b i a n ( I r 0 S 3 , q ) ; I v S 3=I J S 3 ∗ q d + d i f f ( I r 0 S 3 , t ) ; I j bar S3 = j a c o b i a n ( I v S 3 , q )∗ q d + d i f f ( I v S 3 , t ) ; K3 omega IK3 = [ 0 ; 0 ; p h i 3 d ] ; K J R3 = j a c o b i a n ( K3 omega IK3 , q d ) ; K j bar R3 = j a c o b i a n ( K3 omega IK3 , q )∗ q d + d i f f ( K3 omega IK3 , t ) ;

36 37 38 39 40 41 42 43 44

% a p p l i e d f o r c e s /moments : I f 1 = m∗ g ; I f 2 = m∗ g ; I f m p1 = ( phi1 )∗ c r o t + m p2 = ( p h i 2 −p h i 1 )∗ c r o t + m p3 = ( p h i 3 −p h i 2 )∗ c r o t + K1 m 1 = [ 0 ; 0 ; −m p1+m p2 ] ; K2 m 2 = [ 0 ; 0 ; −m p2+m p3 ] ; K3 m 3 = [ 0 ; 0 ; −m p3 ] ;

3 = m∗ g ; ( phi1 d )∗ d r o t ; ( p h i 2 d −p h i 1 d )∗ d r o t ; ( p h i 3 d −p h i 2 d )∗ d r o t ;

45 46 47 48 49 50 51 52 53 54 55 56 57

% e q u a t i o n s o f motion : M = simplify ( . . . m∗ I J S 1 . ’ ∗ I J S 1 + K J R1 . ’ ∗ K I S ∗ K J R1 + . . . ; m∗ I J S 2 . ’ ∗ I J S 2 + K J R2 . ’ ∗ K I S ∗ K J R2 + . . . ; m∗ I J S 3 . ’ ∗ I J S 3 + K J R3 . ’ ∗ K I S ∗ K J R3 ) ; h = simplify ( . . . − I J S 1 . ’ ∗ (m∗ I j b a r S 1 −I f 1 ) − K J R1 . ’ ∗ ( K I S ∗ K j b a r R 1 + t i l d e ( K1 omega IK1 )∗ K I S ∗ K1 omega IK1−K1 m 1 ) . . . − I J S 2 . ’ ∗ (m∗ I j b a r S 2 −I f 2 ) − K J R2 . ’ ∗ ( K I S ∗ K j b a r R 2 + t i l d e ( K2 omega IK2 )∗ K I S ∗ K2 omega IK2−K2 m 2 ) . . . − I J S 3 . ’ ∗ (m∗ I j b a r S 3 −I f 3 ) − K J R3 . ’ ∗ ( K I S ∗ K j b a r R 3 + t i l d e ( K3 omega IK3 )∗ K I S ∗ K3 omega IK3−K3 m 3 ) ) ;

... ... ...

Table 4: Description of Listing 9 line: 1 to 5 7 to 9

code: setting of parameters definition of symbolic variables necessary for the following derivations (phi1 → ϕ1 , phi1_p → ϕ˙ 1 , ...) 11 to 17 derivation of kinematics for body 1: • the velocities I v Si are calculated with the time derivatives of the position vectors I v Si = I r˙ Si , since t is not expressed explicitly in I r OSi (t, q), the ∂r ∂r , q˙ + time derivative is calculated with v = r˙ = dtd r(q, t) = ∂q ∂t |{z} |{z} ιˆ J

19 28 37 46

to to to to

26 35 44 57

• using the same argument ¯ι for translational and rotational motion is cal∂r ∂v ∂v ˙ t) = culated with: a = v˙ = dtd v(q, q, q¨ + q˙ + ∂q ∂q ∂t {z } |{z} | ι¯ J

same procedure for body 2 same procedure for body 3 calculation of applied forces and moments ¨ − h(t, q, q) ˙ = 0 corresponding to derivation of equation of motion M (t, q) q Equation 12

To demonstrate the modeling of a force law that may not be expressed symbolically, the beam model is extended with a unilateral contact with a spring. Figure 5 shows the example with the spring cP4 applying a vertical force on body 3 at point P4 if its vertical position is under a certain level. The Matlab code needs a couple of small modifications: 1. The derivation of applied forces and moments (Listing 9, line 37 to 44) is extended by a force f_P4 applied in P4 in vertical direction on body 3 (see Listing 10). 2. The derivation of function handles is extended by a routine I_r_0P4 for the calculation of the position of P4 . The function for calculating h(t, x) is extended to h(t, x, fP4 ) because the value of h now depends on the force fP4 (see Listing 11). 3. For the calculation of static equilibrium a modified routine is used considering the nonlinear force element (see Listing 12). 4. For the numerical integration a modified routine is used that considers the nonlinear force element (see Listing 13).

Listing 10: Symbolic Derivation of Applied Forces and Moments for Example with Unilateral Spring (replace line 37 to 44 of Listing 9) 1 2 3 4 5 6 7 8 9 10

% a p p l i e d f o r c e s /moments : syms f P 4 ; I f P 4 = [ 0 ; f P 4 ; 0 ] ; K3 r S3P4 = [ l / 2 ; 0 ; 0 ] ; I r 0 P 4 = I r 0 S 3 + A IK3 ∗ K3 r S3P4 ; I f 1 = m∗ g ; I f 2 = m∗ g ; I f 3 = m∗ g + I f P 4 ; m p1 = ( phi1 )∗ c r o t + ( phi1 d )∗ d r o t ; m p2 = ( p h i 2 −p h i 1 )∗ c r o t + ( p h i 2 d −p h i 1 d )∗ d r o t ; m p3 = ( p h i 3 −p h i 2 )∗ c r o t + ( p h i 3 d −p h i 2 d )∗ d r o t ; K1 m 1 = [ 0 ; 0 ; −m p1+m p2 ] ; K2 m 2 = [ 0 ; 0 ; −m p2+m p3 ] ; K3 m 3 = [ 0 ; 0 ; −m p3 ] + t i l d e ( K3 r S3P4 )∗ A IK3 . ’ ∗ I f P 4 ;

Listing 11: Derivation of Function Handles Extended with Contact Force 1 2 3 4 5 6 7 8 9 10 11

f f f f f f f f f f f

. r o t {1} . r o t {2} . r o t {3} . pos {1} . pos {2} . pos {3} .h .M . I r 0P4 . ode fcn . equ fcn

= = = = = = = = = = =

sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo sym 2 fun ( coo @elastic beam @elastic beam

2 v e c ( A IK1 , x ) , ’x , t ’ ) ; 2 v e c ( A IK2 , x ) , ’x , t ’ ) ; 2 v e c ( A IK3 , x ) , ’x , t ’ ) ; 2 vec ( I r 0P1 , x ) , ’ x , t ’ ) ; 2 vec ( I r 0P2 , x ) , ’ x , t ’ ) ; 2 vec ( I r 0P3 , x ) , ’ x , t ’ ) ; 2 vec (h , x ) , ’x , t , f P4 ’ ) ; 2 v e c (M, x ) , ’x , t ’ ) ; 2 vec ( I r 0P4 , x ) , ’ x , t ’ ) ; ode fcn with contact ; equilibrium fcn with contact ;

Listing 12: Calculation of Static Equilibrium Extended with Contact Force 1 2 3 4 5 6 7 8

f u n c t i o n n u l l=e l a s t i c b e a m e q u i l i b r i u m f c n w i t h c o n t a c t ( q , f ) c P4 = 1 0 0 0 0 0 ; f P4 = 0; x = [ q 0 0 0 ] ; I r 0P4 = f . I r 0P4 (x , 0 ) ; i f I r 0 P 4 (2)