SIMULATION OF OPEN-LOOP MULTIBODY SYSTEMS WITH TIME-VARYING TOPOLOGY:

Transcription

1 Proceedings of MUSME 2005, the International Symposium on Multibody Systems and Mechatronics Uberlandia (Brazil), 6-9 March 2005 Paper n.xx-musme05 SIMULATION OF OPEN-LOOP MULTIBODY SYSTEMS WITH TIME-VARYING TOPOLOGY: A RECURSIVE HAMILTONIAN APPROACH Joris Naudet Department of Mechanical Engineering Vrije Universiteit Brussel (VUB) Pleinlaan 2, 1050 Brussels, Belgium Joris.Naudet@vub.ac.be Dirk Lefeber Department of Mechanical Engineering Vrije Universiteit Brussel (VUB) Pleinlaan 2, 1050 Brussels, Belgium dlefeber@vub.ac.be eywords. Hamiltonian equations, Recursive formulation, Forward dynamics, Time-varying topology 1 INTRODUCTION Multibody systems (MBS) dynamics is the study of the motion of systems of interconnected bodies and the forces and torques exerted on them. The simulation of the motion of mechanical systems has a wide variety of applications such as virtual prototyping, virtual reality, computer animation, advanced robot control and predictive display of time-delayed tele-operation systems. One can state that two main challenges are on the focus of research. On one hand, a lot of research has been and is still being done to reduce the computation time of dynamical simulations, while the complexity of the simulated systems keeps growing. The efficiency of simulations can be increased by designing more efficient algorithms for obtaining the equation of motion (Featherstone, 1987; ane & Levinson, 1985; Rosenthal, 1990; Vukobratović et al., 1994; Jerkovsky, 1978; Baraff, 1996; Anderson & Critchley, 2003; Samin & Fisette, 2003) or by achieving better numerical integration (Lankarani & Nikravesh, 1988; Bayo & Avello, 1994; Rodríguez et al., 2004) for advancing the state in time. On the other hand, many efforts are done to incorporate events in the simulations, while retaining computational efficiency. Contact detection and impacts, user-interaction and time-varying topologies need all to be taken into account when realistic simulations are needed, this is especially true for physics based computer games(novodex, 2004). In previous publications (Naudet et al., 2003a; Naudet et al., 2003b), an attempt was made to address the first challenge. A new, canonical momenta based algorithm was presented to solve the forward dynamics problem in a very efficient way, by reducing the number of operations required to obtain the equations of motion. So long, only open-loop structures were considered, as will also be the case throughout the rest of this text. This paper is a contribution to the second challenge: obtaining flexible, interactive simulations. The proposed algorithm seems very suitable, through its recursive nature, for tackling changing topologies in a very flexible way, while keeping the computational efficiency. In next section, the problem of handling time-varying topologies will be addressed. Before discussing the specific details of the method for the recursive Hamiltonian formalism, it will be necessary to give an overview of the basic algorithm. In section 3, it will thus be briefly described. As a matter of fact, it will appear to take the largest part of the article. Tackling changing topologies with the algorithm is indeed not such a complicated problem, although some issues have to be dealt with. It is not the purpose of this paper to introduce a novel simulation technique, but rather to attract the attention of the reader on the potential of recursive algorithms for this particular problem, as literature on this matter seems to be lacking. As joint breaking may be related to the reaction forces in the joint in many cases, a method is needed to compute these reaction forces. This is not a trivial task with the proposed algorithm, section 5 will therefore be dedicated to finding these reactions.

2 2 CHANGING TOPOLOGY The topology of a multibody system is the set of relationships between the components of the system. In other words, it represents the amount, the place and the kind of connections between the different bodies of the system. Some simulations of multibody systems can require dynamically changing topologies, which means the structure of the system changes during the simulation process. This can happen when joints or links are breaking (fig. 1), or in systems where certain degrees of freedom are (artificially) blocked or unlocked. Think, for example, of a robot carrying an articulated object and releasing it at a certain point. (a) Before joint failure. (b) After joint failure. Figure 1: Breaking of a joint. The aim of this paper is to explain how open-loop MBS with time-varying topology can be simulated efficiently, and preferably even in real-time. If the topology of a multibody system changes during the simulation, the governing equations of motion change as well. If the number of possible topologies is limited, it is an option to derive the different equations of motion for each subsystem. One can then use symbolical methods (Samin & Fisette, 2003) to optimize these equations, and switch from one to the other when joint-triggering events occur. This can result in efficient simulations. When the considered multibody system has a large amount of degrees of freedom, requiring many calculations, and the number of possible topologies is not limited, deriving all respective sets of equations explicitly becomes unpractical. To avoid this, one needs a method where the calculations are not entirely altered when a topology change occurs. One way could be to use a descriptor form, where each body is described as an unconstrained body and additional joint constraints are added. Changing a joint has repercussions on the added constraints and the derived Jacobian, but most of the equations stay unchanged (Baraff, 1996). Another approach, the focus of this paper, is the use of recursive algorithms. As each element of the system is treated separately, changing a joint has only direct repercussions on the calculations involved with the body where the change occurs. When the topology changes, only a small part of the equations has to be changed. This can be done by creating a library of joint dependent functions, and calling the appropriate functions for each body. This way, a minimum of (bookkeeping) operations need to be performed when a joint alters. More details about these functions library are given in section 4. 3 FORWARD DYNAMICS OF OPEN-LOOP MBS USING CANONICAL MOMENTA 3.1 Introduction It was claimed above, that the recursive Hamiltonian algorithm is well suited for systems with changing topology. To show this, it is unavoidable to give at least a short description of the algorithm. Therefore, the concepts of Hamiltonian formalism, canonical momenta and articulated momentum vector need to be introduced. The resulting algorithm is of order O(n), which means the number of required computations is linear in the number of bodies. The efficiency can be shown by an example: a multibody system with n components interconnected by pin-joints requires 363n 475 arithmetical operations (table 3.1). A more detailed description of the algorithm can be found in (Naudet et al., 2003a) or (Naudet et al., 2003b).

3 Algorithm Add. Mult. Total (Featherstone, 1987) 275n n n 238 (Vukobratović et al., 1994) 231n n n 566 (Stejskal & Valášek, 1996) 206n n n 688 (Rein, 1993) 195n n n 669 (Naudet et al., 2003a) 178n n n 475 Table 1: Number of required operations for different algorithms 3.2 Hamiltonian formalism The equations of motion of a mechanical system can take many forms, depending on the amount and the nature of the state coordinates. The Hamiltonian equations can be found by applying a Legendre transformation on the well known Lagrangian equations (Goldstein, 1950). This transformation changes the description of the system in terms of generalized coordinates q and velocities q to a description in terms of the same coordinates q and their conjugated canonical momenta p. These canonical momenta are defined as: p = L (1) q They are an extension of the concept of linear and angular momenta to generalized coordinates. L = T V is the Lagrangian function. Applying the Legendre transformation yields q = H p ṗ = H q + Q with the Hamiltonian H = p T q L. These are the equations for unconstrained systems. (2a) (2b) 3.3 Canonical momenta of a rigid body The classical formulation of the Newton-Euler equations for a single free moving body is given by v m d0 G dt d ω J G dt = f (3a) + ω J G ω = t G (3b) The first equation is typically written in an inertial reference frame (notation d0 dt ), while the second is formulated in a frame fixed to the body ( d dt ). The force and the torque that act on the object are represented by f and t. The matrix J is the inertia tensor, m is the mass of the body, ω is the angular velocity referred to the inertial axes and v G the linear velocity of the center of mass (see figure 2). The index G denotes that the momenta and the tensor of inertia are taken with respect to the center of mass. The 6-dimensional spatial momentum vector is defined as follows: ( ) ( ) (v ) pl mi m GO P = = p a mõg J = MΩ (4) ω This is not the same vector as was used in the previous section to denote the canonical momenta p. Inspection of P reveals that it is nothing more than a concatenation of the linear (p l ) and angular (p a ) momenta of the rigid body. I is a unity matrix, v the linear velocity of the origin O of the local reference frame. This origin must lie on the rotational joint axes, if present. J is the tensor of inertia referred to point O. M is called the mass matrix of the rigid body. x is a skew-symmetric matrix constructed from the vector x and is an alternative notation for the cross product. 0 x 3 x 2 a 1 x a = x a = x 3 0 x 1 a 2 (5) x 2 x 1 0 a 3

4 (a) inematics (b) Dynamics Figure 2: Notation on a single rigid body. Ω is the spatial velocity vector. It can be written as a function of the coordinate velocities: Ω = E q (6) E is called the joint matrix. The column vectors of the joint matrix form a basis for the space of virtual motions and are hence orthogonal to the space of the generalized reaction forces. They are the partial derivatives of the spatial velocity vector to the generalized coordinates. The coordinate velocities vector q has dimension n, which is the number of degrees of freedom of the body. The joint matrix E therefore has dimensions 6 n. Instead of trying to find an algorithm directly starting from the Hamiltonian equations, the Newton-Euler equations (3) are reformulated in relative axes, and written with respect to the origin O. Note that the relation between the time derivatives in two different frames and L is given by d L x x = d + ω r x (7) dt dt ω r is the relative angular velocity of frame with respect to frame L. Furthermore, the momentum vector (4) is introduced in the equations. After some mathematical manipulations, and observing that p l = mv G, equations (3) can be reformulated as: ) )( ) ( (ṗl ( ω 0 pl f + = (8) ṗ a ṽ ω p a t) By convention, all angular momenta are taken with respect to the origin O of the local reference frame. ẋ stands for the time derivative in local axes, e.g. ω = d ω dt. This implies that Ṁ = 0. One can go further into the conciseness of the equations, by defining a 6-dimensional cross product as follows: ( ) ) v ( ω 0 Ω = (9) ω ṽ ω The equations of motion for a single rigid body then become Ṗ + Ω P = T (10) with T = ( f T m T) T the spatial force vector, comprising all external and eventual reaction forces. It can easily be shown that the kinetic energy T of a single rigid body can be expressed as: which enables the calculation of the canonical momenta with (1) T = 1 2 ΩT MΩ = 1 2 ΩT P (11) p = L q = T q = ΩT q MΩ = ET MΩ = E T P (12) The canonical momenta p conjugated to the generalized coordinates q are thus the projections of the momentum vector P on the joint axes.

5 3.4 Canonical momenta of a multibody system In a multibody system, the reaction forces between the components need to be taken into account. By taking linear combinations of equations (10) for the different components, one gets: Ṗ + Ω P = T + T r (13) with P the so-called articulated momentum vector and T the accumulated force vector. The joint reaction force T r is written here explicitly. P and T are in fact the momentum vector and the force vector of the rigid body that would be obtained by freezing all joints outboard from body. P = P + i T = T + i T F i P i (14) T F i T i i outboard bodies (15) The force transformation matrix T F N is used here, to transmit the forces and momenta from one joint to another ( ) I 0 T F N = O O N I (16) Note that this matrix is constant in the local reference frame. Observe also that the velocities transform in a similar way: ( ) (v ) Ω N = N T V Ω + E N q N = I O N O + E 0 I ω N q N (17) The relationship between both transformation matrices is given by: T F N = ( N T V ) T (18) One can show that the projection of an articulated momentum vector on its joint subspace results in a set of canonical momenta of the multibody system: 3.5 Equations of motion of a MBS p = E T P (19) As the subspace of the reaction forces is orthogonal to the subspace of the joint matrix, one can eliminate these reactions by projection of (13) on E: This gives half of the Hamiltonian equations. 3.6 Joint velocities ṗ = E T j T E T (Ω P ) + ĖT P (20) The other half of the Hamiltonian equations can be found writing the articulated momentum vector as (Naudet et al., 2003b) with P = M Ω + D (21) M =M + T F +1M +1+1 T V (22) D = T F +1D +1 (23) M j =E T M E (24) M =M M E M 1 j E T M (25) D =M E M 1 j (p E T D ) + D (26)

6 M is the articulated mass matrix, M is called the reduced or projected mass matrix, M j is the joint space mass matrix,d the remainder momentum vector and D the reduced remainder vector. The coordinate velocities are found as 3.7 Overview of the algorithm q = M 1 j [(p E T D ) E T M T V 1Ω 1 ] (27) The algorithm is made of two recursion steps. During the first, backward recursion, the articulated mass properties and the accumulated forces are calculated. During the second, forward recursion, the velocities and momentum vectors are propagated from base to tip(s). The full equations of motion can then be calculated. The algorithm is written below in pseudo-code. The children to which is referred are all adjacent outboard bodies of the considered body. The recursive nature of the algorithm is emphasized by making recursive calls to the recursion functions. BACWARD RECURSION set i=base body backward recursion step(i) { for (j=firstchild,j==lastchild,j++) { backward recursion step(j); } compute articulated mass matrix(i); compute remainder momentum vector(i); compute reduced mass matrix(i); compute reduced remainder vector(i); compute accumulated force vector(i); } FORWARD RECURSION set i=base body forward recursion step(i) { compute coordinate velocity(i); compute spatial velocity(i); compute momentum vector(i); compute time derivative of canonical momenta(i); for (j=firstchild,j==lastchild,j++) { forward recursion step(j); } } 4 CHANGING TOPOLOGY AND RECURSIVE HAMILTONIAN FORMALISM When looking at the algorithm in section 3.7, one can see that the same kind of functions are called recursively for each body. Independently on the kind of joint, one has to compute the articulated mass matrix and the remainder momentum vector, the accumulated force vector, the articulated momentum vector and the spatial and joint velocities. Moreover, same joints require exactly the same computations. It is therefore a sensible step to write separate functions for each of these quantities and for each possible joint, creating a function library by doing so. Some functions are not joint dependent, but topology dependent. The articulated mass matrix (22) for example, is the sum of the mass of the considered body and the transformed reduced mass matrices ( T F +1M +1+1 T V ) of its children. While the transformed reduced mass matrices are joint dependent and are quantities related belonging to the children, the summation itself to obtain the articulated mass matrix is only topology dependent. At each time step of the simulation process, the equations of motion are established. During the backward and forward recursion steps, the simulator runs through the links and checks the kind of joint associated with it. According to that joint, the adequate functions from the library are called. For the topology dependent functions, the simulator only needs to look for the children of the link and fetch the related transformed quantities.

7 4.1 Practical issues Bookkeeping The primary task of the simulator is to calculate the evolution of the subsystems s states. It must however also keep track of the topology changes. When an event occurs, resulting in such a change, the simulator has a few operations to execute. It first must change the list of children of the body where the change happens and define a new subsystem, if needed. This can all be implemented very efficiently by making judicious use of tree data structures. A few assumptions are made according to the topology change: the change happens in a negligible time lapse and the configuration (coordinates) nor the velocities do change during that time. The change however, is likely to influence the values and the numbers of canonical momenta conjugated to the considered joint. This is described below Changes in canonical momenta for partial joint failure As was seen before, the canonical momenta can be obtained from the articulated momentum vectors: p = E T P (28) During a topology change, some of the joint matrices E transform, alternating the size and values of vectors p. The articulated momentum vectors however do not change, so only the canonical momenta conjugated to the body where the joint failure occurs change. This requires very few computations Changes in canonical momenta for full joint failure When a joint breaks completely, two options can be considered. One can handle the break as a new 6D-joint and express the configuration of the disconnected part relative to the remaining part, which leads to the situation in previous paragraph. On the other hand, one can also treat the two new subsystems as fully independent. In that case, the articulated momentum vectors do change. Indeed: P = P + i OB T F i P i (29) The summation is taken over all outboard bodies (OB). If the chain is not broken, the summation remains unchanged, otherwise, less bodies contribute to the articulated momentum vector. Note that the absolute configuration need to be known to calculate the new coordinates Function libraries As said previously, each kind of joint requires a specific set of functions, like for the computation of the reduced mass matrix M, the reduced remainder term D, the joint velocity q,... It it a tedious work to create this library, but this has only to be done once (off-line) and, once obtained, any kind of (open-loop) multibody can be simulated. The functions can best be optimized by using symbolical methods (Samin & Fisette, 2003), mainly eliminating all calculations with 0 and Augmentation and reduction of the number of degrees of freedom An augmentation of the number of degrees of freedom(dof) happens without impact. It does however introduce additional joint coordinates. A reduction of the number of DOF on the other side, which can happen when locking a joint, is usually combined with an impact. Therefore, momentum changes have to be calculated through the impact equations. These are not described in this paper, but they are very similar to the equations for the forward dynamics. 4.2 Numerical example A numerical example has been worked out to give an idea of the distribution of computer time on the different tasks (simulation before topology change, handling the joint failure, simulation after the change). The considered example is a planar chain of 100 bodies connected to each other by pin-joints. All elements are the same, they have a mass of 10 grams and an inertia of 0.83g.cm 2. The distance between two consecutive joints is 5cm. A viscous friction coefficient of Ns m is chosen for each joint and the chain moves under the influence of gravity. A starting relative angle of 0.001n is taken, n being the number of the joint counting from the base body. The only

8 Processing times Time for function evaluations Total simulation 0.64s 5.40s Simulation till joint failure 0.39s 5.23s Handling topology change 0.00s 0.04s Table 2: Processing times. motivation for this choice is to get an appealing simulation. The system is simulated during 3.0s, a Runge-utta of fourth order method with a fixed timestep of 0.001s is taken for the numerical integration. After 1.8s, the joint at the 41st body is broken, splitting the system in a smaller chain of 40 bodies and another free falling one of 60 bodies. The accuracy of the simulation has not been investigated, as this is not the focus of this paper. The simulation is run on a 2.0GHz Pentium4 computer with 256Mb of RAM, the program is written under C++. The simulation is showed on figure 4.2. One frame is given for every 0.2s simulation time, the small disk denotes the connection of the first body to the fixed environment. Figure 3: Simulation of a breaking chain. Table 2 contains the processing times for the different parts of the simulation. The processing time for function evaluations is also given, for better comparison. For the actual simulation parts, the function which is referred to is the function returning the time derivatives of the coordinates and their conjugated momenta. This function is called 4 times each timestep when using the above mentioned Runge-utta method. The slight processing difference for the two parts of the simulation can be explained by the fact that less calculations have to be performed for the unconnected body. As for the topology change, the function refers to the calculation of the new coordinates and new canonical momenta and performing some bookkeeping operations. It is called only once during the whole simulation. One should keep in mind that the numbers given in the table are strongly dependent on which computer is used and may differ widely even on comparable chipsets. The relative computation times are far more important. As one would expect, the computational load is approximately the same for the simulation before and after the joint failure. The topology change itself is tackled in a negligible time lapse. 5 JOINTS ACCELERATIONS AND REACTION FORCES/TORQUES If joint reactions forces/torques are a criteria for topology change, these quantities need to be calculated. Unfortunately, this cannot be done straightforwardly with the recursive form of Hamiltonian equations. A method to obtain these spatial reaction forces is described below.

9 5.1 Reaction forces The equations of motion for a rigid body interconnected with other bodies is given by: Ṗ + Ω P = T + T r T +1 T r+1 (30) When using articulated bodies, the reaction forces and torques introduced by the outboard bodies are eliminated, as told before: Ṗ + Ω P = T + T r (31) If the time derivative of the articulated momentum vector is known, the spatial joint reaction force can be computed: T r = Ṗ + Ω P T (32) As will be shown in following section, the time derivative of the articulated momentum vector can be found by means of one backward and one forward recursion step. In the same process, the joint accelerations can be found. 5.2 Time derivative of the articulated momentum vector The method for obtaining the joint accelerations and the time derivative of the momentum vectors is quite similar to the algorithm for the forward dynamics. First take a look at the momentum vector of the most outboard body N. Taking the time derivative results in: P N = M N Ω N = M N ( N T V N 1Ω N 1 + E N q N ) (33) Ṗ N = M N [ N T V N 1 Ω N 1 + (E N q N I) T N T V N 1Ω N 1 + E N q N ] (34) from which the expression for the joint accelerations follows Substitution in (34) finally gives: q N = M 1 j N E T N[ṖN M N (E N q N I) T N T V N 1Ω N 1 M N N T V N 1 Ω N 1 ] (35) with Ṗ N = M N[ N T V N 1 Ω N 1 + (E N q N I) T N T V N 1Ω N 1 ] + M N E N M 1 j N ṗ N (36) = M N N T V N 1 Ω N 1 + D N (37) D N = M N(E N q N I) T N T V N 1Ω N 1 + M N E N M 1 j N ṗ N (38) The only unknown in this expression is the spatial acceleration Ω N 1. It will be computed in a forward recursion step, using Ω = T V 1 Ω 1 + (E q I) T T V 1Ω 1 + E q (39) Note that the reduction of the spatial velocity of body N 1 to its outboard body N is already calculated in the derivation of the equations of motion. The articulated momentum vector of body = N 1 is given by Thus, its time derivative is P = P + T F +1P +1 (40) Ṗ = Ṗ + T+1 F d P +1 + d T+1 F P +1 (41) dt dt = M Ω + T+1(E F +1 q +1 P +1) + T +1Ṗ F +1 (42) = M Ω + T F +1(E +1 q +1 P +1) + T F +1(M T V Ω + D +1) (43) = M Ω + D (44)

10 with D = T F +1 D +1 + T F +1(E +1 q +1 P +1 ) (45) One can use the same procedure as above to find the other joint accelerations: Ṗ = M [ T V 1 Ω 1 + (E q I) T T V 1Ω 1 + E q ] + D (46) from which the expression for the joint acceleration follows q = M 1 j E T [Ṗ D M (E q I) T T V 1Ω 1 M T V 1 Ω 1 ] (47) Substitution in the articulated momentum vector for further recursions gives: with Ṗ = M [ T V 1 Ω 1 + (E q I) T N T V 1Ω 1 ] (48) +M E M 1 j (ṗ d ) + D (49) = M T V 1 Ω 1 + D (50) D = M (E q I) T T V 1Ω 1 + M E M 1 j (ṗ d ) + D (51) This procedure can be carried on until the base body is reached, which velocity is known. 6 CONCLUSIONS It was shown in this article how a recursive algorithm based on the Hamiltonian formalism can be used to cope with topology changes with almost no extra computing overhead. As topology changes may be triggered by high reaction forces/torques, a method is described to obtain joint accelerations and joint reactions recursively. 7 FUTURE WOR The algorithm to obtain the equations of motion has recently be extended to cope with closed-loop systems while keeping the efficiency. The topology changes can then also include the creation of new constraints (e.g. as in walking robots). 8 ACNOWLEDGMENT This research is supported by the Institute for the Promotion of Innovation by Science and Technology in Flanders (Belgium). REFERENCES9 References Anderson,. & Critchley, J. (2003). Order-N Performance Algorithm for the Simulation of Constrained Multi- Rigid-Body Dynamic Systems. Multibody System Dynamics, 9, Baraff, D. (1996). Linear-Time Dynamics using Lagrange multipliers. Computer Graphics Proceedings, Annual Conference Series, (pp ). Bayo, E. & Avello, A. (1994). Singularity-Free Augmented Lagrangian Algorithms for Constrained Multibody Dynamics. Nonlinear Dynamics, 5, Featherstone, R. (1987). Robot Dynamics Algorithms. luwer Academic Publishers. Goldstein, H. (1950). Classical Mechanics. Addison-Wesley Publishing Company. Jerkovsky, W. (1978). The Structure of Multibody Dynamics Equations. Journal of Guidance and Control, 1(3), ane, T. & Levinson, D. (1985). Dynamics: Theory and Applications. McGraw-Hill. Lankarani, H. & Nikravesh, P. (1988). Application of the Canonical Equations of Motion in Problems of Constrained Multibody Systems with Intermittent Motion. Advances in Design Automation, 1,

11 Naudet, J., Lefeber, D., & Terze, Z. (2003a). Forward Dynamics of Open-Loop Multibody Mechanisms Using an Efficient Recursive Algorithm Based on Canonical Momenta. NATO ASI Workshop on Virtual Nonlinear Multibody Systems, 10(1), Naudet, J., Lefeber, D., & Terze, Z. (2003b). General Formulation of an Efficient Recursive Algorithm based on Canonical Momenta for Forward Dynamics of Open-Loop Multibody Systems. ECCOMAS Thematic Conference on Multibody Systems, (pp ). Nikravesh, P. & Gwanghun, G. (1993). Systematic Construction of the Equations of Motion for Multibody Systems Containing Closed inematic Loops. Journal of Mechanical Design, 115, Novodex (2004). Rein, U. (1993). Efficient Object Oriented Programming of Multibody Dynamics Formalisms. Proceedings of the NATO-Advanced Study Institute on Computer Aided Analysis of Rigid and Flexible Mechanical Systems, 2, Rodríguez, J., Jiménez, J., Funes, F., & de Jalón, J. (2004). Recursive and Residual Algorithms for the Efficient Numerical Integration of Multi-Body Systems. Multibody System Dynamics, 11(4), Rosenthal, D. (1990). An Order n Formulation for Robotic Systems. The Journal of the Astronautical Sciences, 38(4), Samin, J.-C. & Fisette, P. (2003). Symbolic Modeling of Multibody Systems. luwer Academic Publishers. Stejskal, V. & Valášek, M. (1996). inematics and Dynamics of Machinery. Marcel Dekker. Vukobratović, M., Filaretov, V., & orzun, A. (1994). A unified approach to mathematical modelling of robotic manipulator dynamics. Robotica, 12,