**3. Multibody Formalism**

Among the modeling options presented in Section 2.1, we have opted for a relative coordinate formalism (Section 3.1) and a symbolic implementation of multibody equations (Section 3.2). This combination offers the advantage of producing models that can be easily interfaced, either with other disciplines (in a multiphysics perspective) or within a middelware platform (to couple devices and software in a real-time process).

#### *3.1. Modeling MBS Using Relative Coordinates*

When choosing relative coordinates, a MBS can be built on the basis of its topology characterized by the successive bodies and joints, as shown in Figure 2, on the left for a tree-like MBS (such as a serial robot) and on the right for a MBS containing kinematic loops (such as a multi-link car suspension). The relative angular (*rad*) or linear (*m*) displacements within the joints are called the generalized coordinates, *q*, of the MBS (also called *joint coordinates*). These are independent for a tree-like MBS but not for a closed-loop one, each loop of bodies leading to geometrical constraints, denoted as *h*(*q*) = 0, between the joint coordinates present in the loop. The distinction between tree-like and closed-loop MBS is essential because it is at the root of the relative coordinate formalisms as synthetically explained below.

**Figure 2.** Tree-like (**left**) and closed-loop (**right**) MBS topologies.

#### Tree-Like MBS Modeling

Basically (see in [8] for more details), in the case of a tree-like MBS containing *n* joint coordinates, these confer *n* motion degrees of freedom (DOF) to the system, leading to a set of *n* equations of motion.

Various techniques can be envisaged to obtain them (d'Alembert principle, Lagrange equations, Newton–Euler equations recursively computed on the topology [8]). In any case, one ends up with a form equivalent to the following one ("." stands for *<sup>d</sup> dt* and ".." for *<sup>d</sup>*<sup>2</sup> *dt*<sup>2</sup> ):

$$M(q)\,\ddot{q} + c(q, \dot{q}, frc, trq) = Q(q, \dot{q}),\tag{1}$$

in which *q*˙ and *q*¨, respectively, stand for the generalized relative velocities and accelerations; *M*(*q*) represents the system mass matrix; *c*(*q*, *q*˙) gather the centripetal, Coriolis, gyroscopic terms as well as the external forces *f rc*(*q*, *q*˙) and torques *trq*(*q*, *q*˙); and *Q*(*q*, *q*˙) denotes the efforts within the joints (actuators or passive elements).

Equation (1) can be straightforwardly solved using linear algebra to obtain the relative accelerations *q*¨: this refers to *Direct Dynamics*. This computation has a *O*(*n*3) computational complexity due to the factorization of the mass matrix. Among other things, direct dynamics allows, via the time integration of the accelerations, to determine the time history of the positions *q*(*t*) and velocities *q*˙(*t*) and thus to predict the motion of the MBS subjected to internal or external forces, starting from initial conditions *q*(0), *q*˙(0).

As an illustration, direct dynamics is widely used for road and railway vehicles to predict, analyze, or optimize their dynamic performances. Direct dynamics is also used in various driving simulators to make haptic feedback more accurate and realistic (see Section 5.2).

*Inverse Dynamics* is another way of using Equation (1) for applications where the MBS trajectory is known (*q*(*t*), *q*˙(*t*), and *q*¨(*t*)) and for which the unknowns are now the joint forces and torques *Q*(*t*) and not the accelerations anymore. Equation (1) can then be written as follows:

$$Q(q, \dot{q}) = \phi(q, \dot{q}, \ddot{q}, frc, trq) \tag{2}$$

with

$$\phi = M(q)\ddot{q} + c(q, \dot{q}, frc, trq)$$

As the explicit computation of the mass matrix *M* is not necessary anymore to compute *Q* via (2), *φ* can be generated without explicitly computing it, which leads to an efficient *O*(*n*) computational complexity of the formalism.

For instance, inverse dynamics is useful to compute the actuator torques of robots for evaluating a feedforward component in their motion controller.

#### Closed-Loop MBS Modeling

A multibody system can be subject to different types of constraints between its generalized coordinates, *q*, which can be holonomic (i.e., algebraic, like assembly constraints), non-holonomic (expressed at the level of velocities, like for a slip-free rolling), and/or rheonomic (when they depend explicitly on time *t*). In the present context of haptic devices modeling, we will limit ourselves to the holonomic case, focusing on kinematic loop constraints; this does not exclude the case of other algebraic constraints that would be treated in the same way by the following formalism.

When dealing with closed-loop MBS (as depicted on the right of Figure 2), *m* loop constraints between the generalized coordinates *q*, denoted *h*(*q*) = 0, must be solved in parallel to the equations of motion (1) or (2), in which an additional term is required to take the constraint forces and torques into account, according to the well-established Lagrange multipliers technique. For the direct dynamics, the final form reads

$$\begin{array}{rcl}M(q)\;\ddot{q} + c(q, \dot{q}, frc, trq) &=& Q(q, \dot{q}) + f^t(q)\;\lambda\\h(q) = 0 \; ; \; \dot{h}(q, \dot{q}) = I(q)\; \dot{q} &=& 0 \; ; \; \ddot{h}(q, \dot{q}, \ddot{q}) = 0\end{array} \tag{3}$$

And in a similar way, for the inverse dynamics the equations becomes

$$\begin{array}{rcl}Q(q,\dot{q})&=&\Phi(q,\dot{q},\ddot{q},\text{frc},\text{tr}q)-\dot{J}^{t}(q)\,\lambda\\h(q)&=&0\,\text{; }h(q,\dot{q})=\dot{J}(q)\,\dot{q}=0\,\text{; }\dot{h}(q,\dot{q},\ddot{q})=0\end{array} \tag{4}$$

in which *J*(*q*) *<sup>∂</sup><sup>h</sup> <sup>∂</sup>q<sup>t</sup>* denotes the constraint Jacobian matrix and *λ* are the Lagrange multipliers associated with the constraints *h*(*q*). In both cases, the number of DOF equals *n* − *m*, that is the number of generalized coordinates *q* minus the number of independent constraints *h* (The case of redundant constraints is treated within Robotran, but only for "classical" multibody simulations, and not for the proposed haptic coupling. As there were no redundant constraints for the applications treated in this paper, this topic has not been discussed.).

Let us illustrate closed-loop morphologies in the field of robotics. An anthropomorphic serial robot with 6 actuated joints (Figure 3 (left)) possesses 6 DOFs. On the other hand, a parallel structure such as the Stewart platform (Figure 3 (right)) has 18 joints embedded into multiple loops of bodies, but its upper-plate only possesses 6 DOFs and its motion is ensured by 6 actuators forming the legs of the platform.

**Figure 3.** Serial (**left**) versus parallel (**right**) manipulators.

Equations (3) or (4) form a DAE system, as they are made of differential and algebraic equations, whose direct solving is not trivial. A practical and elegant way to solve them is to transform them into ordinary differential equations (ODE), using the so-called *Coordinate Partitioning Method* briefly presented below (see in [4,8] for more details).

The starting point of the method refers to the number of degrees of freedom (*n* − *m*) of the system, to which (*n* − *m*) generalized coordinates can match. They are called *independent* coordinates, denoted *u*, while the others, denoted *v*, are *dependent*. Therefore, we can partition the column vector *q* as follows:

$$
\eta = \begin{pmatrix} u \\ v \end{pmatrix} \tag{5}
$$

By applying this partitioning to all the matrices and vectors appearing in Equation (3) or (4), and by expressing the dependent coordinates via the solving of the constraints at position, velocity and acceleration levels,

$$\begin{aligned} h(q) &= 0 \quad \Rightarrow \quad v = f(u), \\ \dot{h}(q, \dot{q}) &= 0 \quad \Rightarrow \quad \dot{v} = g(q, \dot{u}), \\ \dot{h}(q, \dot{q}, \ddot{q}) &= 0 \quad \Rightarrow \quad \ddot{v} = h(q, \dot{q}, \ddot{u}), \end{aligned} \tag{6}$$

it is possible, after a few matrix manipulations, to end up with a reduced set of equations of motion in terms of the independent coordinates *u*:

$$\left(M\_{\mathbf{r}}(\mathbf{u}), \ddot{\mathbf{u}} + \mathbf{c}\_{\mathbf{r}}(\mathbf{u}, \dot{\mathbf{u}}, \mathrm{frc}, \mathrm{trq})\right) \\ \quad = \quad Q\_{\mathbf{r}}(\mathbf{u}, \dot{\mathbf{u}}) \tag{7}$$

$$\mathcal{Q}\_r(\mathfrak{u}, \mathfrak{u}) \quad = \quad \mathfrak{\phi}\_r(\mathfrak{u}, \mathfrak{u}, \mathfrak{u}), \tag{8}$$

for the direct and inverse dynamics, respectively.

¨

The coordinate partitioning technique amounts to projecting the equations of motion in the MBS motion manifold, with the advantage of getting rid rigorously of the algebraic constraint equations. Therefore, one ends up with a ODE system of minimum size (n-m) which exactly corresponds to the number of DOF of the MBS (ex. *n* − *m* = 6 for the Stewart platform of Figure 3).

It is worth mentioning that the resolution of the constraints (6) can be very accurate using an iterative (for *h*) or an algebraic (for ˙ *h* and ¨ *h*) algorithm, at each evaluation of the dynamic equations.

#### Driven Motion

When dealing with MBS in which the time evolution of some of the generalized coordinates *u* is fully imposed (such as for instance in robotics when motion control is achieved), the corresponding joints are qualified as *driven*. The time history of the associated coordinates is thus a prescribed function of time *t*. To take them into account in the above formalism, we can apply a second partitioning, but on *u* in the present case, that is,

$$
u = \begin{pmatrix} u\_{\tt n} \\ u\_d \end{pmatrix}'\tag{9}$$

in which *uu* represents the new subset of independent coordinates and *ud* stands for the driven coordinates viewed, from the MBS point of view, as known functions of time at position, velocity, and acceleration levels:

$$
\mu\_d = f(t) \; ; \; \dot{u}\_d = \frac{d u\_d}{d t} \; ; \; \dot{u}\_d = \frac{d^2 u\_d}{d t^2} \tag{10}
$$

Note that driven variables can also be used to deal with rheonomic constraints. Indeed, they allow to describe an explicit time dependence of any function in which they appear, as in an algebraic constraint for example.

Introducing the partitioning (9) into the equations of motion (7) gives

$$
\begin{pmatrix} M\_r^{\text{mu}} & M\_r^{\text{mu}} \\ M\_r^{\text{du}} & M\_r^{\text{dd}} \end{pmatrix} \begin{pmatrix} \ddot{u}\_{\text{ll}} \\ \ddot{u}\_{\text{d}} \end{pmatrix} + \begin{pmatrix} c\_r^u \\ c\_r^d \end{pmatrix} = \begin{pmatrix} Q\_r^u \\ Q\_r^d \end{pmatrix} \tag{11}
$$

The first line of (11) can be solved with respect to *u*¨*<sup>u</sup>* and represents the final form of a *direct dynamics* problem (to be time integrated with respect to the *uu* variables for instance):

$$M\_r^{uu}\ddot{u}\_{\iota} + \mathfrak{c}\_r^u = Q\_r^u - M\_r^{ud}\ddot{u}\_d \tag{12}$$

Let us note that this equation can also be formulated in an inverse dynamics form, in a similar way to Equation (8).

The second line of (11) can be put aside if we are only interested in analysing the motion of the MBS Equations (12). However, those equations can be very useful to compute efforts (*Q<sup>d</sup> <sup>r</sup>* ) associated with the driven coordinates (ex. a motor torque, a force in an mechanical assembly or a reaction force in a bearing):

$$\mathbf{Q}\_r^d = \mathbf{M}\_r^{du}\mathbf{i}\_u + \mathbf{M}\_r^{dd}\mathbf{i}\_d + \mathbf{c}\_r^d \tag{13}$$

In a certain manner, this equation represents an inverse dynamics computation carried out in parallel with—or as a postprocess of—the direct dynamics (12).

#### Implementation in a Haptic Context

The multibody computation workflow varies slightly whether direct dynamics or inverse dynamics are considered (Figure 4). For direct dynamics simulations, a numerical integrator computes the time evolution of the independent variables. Trajectory sensors (i.e., position, velocity, or acceleration sensors) define the motion of *driven joints* (Step A), such as the angular position of a steering wheel for a driver simulator or the displacement of a piano key (see Section 5). When the trajectories of all independent joints are measured, the inverse dynamics formulation is used. Filtering, differentiating or integrating the sensor signal may be necessary.

The algebraic constraints (6) of closed-loop MBS are used to compute dependent positions and velocities (step B). Once the configuration of the system is fully defined, external and internal forces are computed, for instance, the tyre/ground interaction for a car or the contact interaction between the parts of a piano action (step C). At this stage, inputs coming from force/torque sensors can be considered in the dynamics. The dynamic Equations (3) or (4) are computed and, for closed-loop systems, the coordinate partitioning technique is applied (step D) to get the reduced set of Equation (7). In the case of direct dynamics, a linear system of equations must be solved (step E) to compute the independent

acceleration that will be handled by the time integrator. For the inverse dynamics, joint forces/torques are derived directly from the dynamics (step F) using (8).

Forces or torques associated with driven joints are computed (step F) using (13), which enables computing a feedback that takes the dynamics into account, like the wheel steering torque on the driver or the reaction of the piano key on the musician finger. In addition, any kinematic information of the system, such as the position or velocity of any point, can also be computed (step G) and be used for the system feedback using so-called *multibody sensors*.

Direct dynamics Inverse dynamics

**Figure 4.** Flow diagram of direct and inverse dynamics in view of coupling them with haptic devices.

### *3.2. Symbolic Multibody Model*

The aim of the symbolic generation of multibody models is to take advantage of both the genericity inherent to a numerical generation of multibody equations and of the simplification potentialities of their manual writing. So-called *symbolic* multibody programs manipulate arithmetical operators (+, −, \*, /) and alphanumeric strings representing the data (*m*, *c*, *K*, etc., see Figure 5) to generate analytical equations in C, Python, or MATLAB, making the most of arithmetical and trigonometric simplifications. For a generic 3D MBS, the symbolic generation is performed only once (in less than one second with ROBOTRAN for MBS containing a hundred DOF) prior to any numerical analysis.

**Figure 5.** Symbolic Generation Process.

The computational superiority of the symbolic vs. the numerical approach is particularly attractive since it gives the opportunity to


These two advantages are widely exploited in the applications underlying this work as detailed in the next sections.

#### ROBOTRAN Symbolic Generation

This section summarizes the key elements of the symbolic generation underlying the multibody software ROBOTRAN, the details of which can be found in reference [10], dedicated to this precise topic. There do obviously exist commercial general-purpose symbolic packages: why do we not use these to generate multibody equations?

The first reason relates to the amount of computer memory required to generate large symbolic models (up to 400 DOFs in our case). The combined use of dynamic memory allocation (via the use of C-pointers) and of the linked lists technique, allows us to finely control the ROBOTRAN memory requirement during the generation process.

The second reason concerns the possible simplifications of mathematical alphanumeric expressions. Although the simplification capabilities of commercial packages are extremely powerful, the symbolic condensation of multibody equations relies on a set of rules and tricks which are specific to MBS, namely, the simplification of complex trigonometric formulae and kinematic expressions, the pure and lasting elimination of unnecessary recursive equations and the parallelization of the latter, among others.
