A general framework for modeling and dynamic simulation
of multibody systems using factor graphs

For any given MBS with $f$ dofs, the use of $n$ dependent coordinates expressed by the vector ${\mathbf{q}}$ requires $m=n-f$ constraint equations, which form the following set of equations

Thus, it is assumed that there are at least as many equations as there are unknown coordinates. To solve the kinematic problem, the time derivative of Eq. (1) is required, one time for velocity analysis and two times for acceleration analysis, leading to the following set of equations

where $\dot{\mathbf{q}}$ is the vector of dependent velocities, $\mathbf{\Phi_{q}}\in\mathbb{R}^{m\times n}$ the Jacobian matrix of Eq. (1) respect to ${\mathbf{q}}$, and ${\boldsymbol{\Phi}}_{t}$ the time derivative of constraint equations that is equal to the null vector for time-independent constraints.
To solve the dynamic problem, external forces and inertia forces need to be considered. From the classical Newton’s law, one obtains the following set of differential equations that expresses the force equilibrium equations

where $\boldsymbol{\lambda}$ is the vector of Lagrange multipliers, ${\mathbf{M}}$ is the system mass matrix and vector ${\mathbf{Q}}$ contains the generalized external forces. Since Eq. (4) is a system of $n$ equations in $n+m$ variables, by adding the $m$ equations (3) one obtains the following system

Normally [20, 21], this equation is solved for the extended vector of unknowns that includes both, ${\ddot{\mathbf{q}}}$ and $\boldsymbol{\lambda}$. In our framework, we are specifically interested in the generalized accelerations ${\ddot{\mathbf{q}}}$. Therefore, by applying the block matrix inversion lemma (see Appendix I. Block matrix inversion lemma) to Eq. (5) and keeping the first row only, it leads to the following equation of motion expressed in terms of dependent coordinates:

By differentiating Eq. (9) and by taking into account Eq. (3), one gets

From Eq. (4), pre-multiplied by ${\mathbf{R}}^{\top}$

and by substituting ${\ddot{\mathbf{q}}}$ in Eq. (12)

Introducing the reduced mass matrix $\overline{\mathbf{M}}={\mathbf{R}}^{\top}{\mathbf{M}}{\mathbf{R}}$ and force vector $\overline{\mathbf{Q}}={\mathbf{R}}^{\top}\left({\mathbf{Q}}-{\mathbf{M}}\mathbf{Sc}\right)$, the following equation of motion in terms of independent coordinates can be finally obtained

It is instructive to start our discussion by considering a Dynamic Bayesian Network (DBN), a type of graphical model where variables are represented as nodes and directed edges stand for causal relationships [30, 22]. The existence of directed edges in a DBN allows us to encode an expert’s knowledge in causality relationships between all involved variables in the graph.

The rules to convert a DBN into the kind of graphs we are actually using in this work, factor graphs (FGs), are discussed in section 5. A relevant point regarding such conversion is that one single DBN can be mapped into different FGs depending on which subset of the original DBN variables are known data and which unknowns are required to be found. Let us remark that whereas a DBN displays all variables as nodes, in a FG only unknown variables are variable nodes.

Factor graphs (FGs) are bipartite graphs comprising two kinds of nodes: variable nodes, and factor nodes. Variables are the unknown data to be estimated, and factors represent any constraint (a cost function to be minimized) or relationship between variables. A crucial aspect of a FG is its sparsity: each factor node is only connected to the variables that appear in its cost function. Sparse graph optimization algorithms exist with computational cost almost linear with the number of edges in a FG, as opposed to, for example, the cubic cost of a naive implementation of Kalman Filtering [31]. Incremental (e.g. [32]) and sliding-window (e.g. [33]) approaches exist as well, enabling the efficient estimation of problems with thousands to millions of variables [34, 35, 36].

Once a FG is formulated for a given problem, estimating the most-likely value of all the unknowns becomes a numerical nonlinear minimization problem, for which very efficient algorithms exist. As a probabilistic estimator, these optimal values can be assigned an uncertainty. In general, retrieving the covariance $\boldsymbol{\Sigma}$ of the estimated variables involves inverting the Hessian of the linearized problem evaluated at the optimal solution, such that $\boldsymbol{\Sigma}=(\mathbf{J}^{\top}{\boldsymbol{\Lambda}}\mathbf{J})^{-1}$ with $\mathbf{J}$ the sparse Jacobian of all constraints (factors), and ${\boldsymbol{\Lambda}}$ the weight matrix representing our level of certainty about each constraint. The matrix inversion operation is, in general, very costly since it is cubic with the problem dimension. However, optimized methods exist for the kind of sparse problem at hand, see for example, [37, §B.4] or [32].

In forward dynamics simulation, we are given a known initial state of a mechanical system (position ${\mathbf{q}}_{0}$ and velocity ${\mathbf{q}}_{0}$), their geometric and inertial properties, and the external forces that act on it (${\mathbf{Q}}_{i}$, $i=0,...,N$). For simplicity, we assume no sensors are installed in the system since this problem instance does not need them, but predicting sensor outputs could be also possible adding the corresponding nodes and factors.

The resulting FG when using dependent coordinates is shown in Figure 2 and it involves the following factor nodes (or plain factors):

• 1.

  $f_{prior}$: Factors imposing a given a priori knowledge about the attached variables, e.g. initial conditions. Prior factors can be defined for both, position and velocity.

• 2.

  $f^{d}_{pc}$: Factor for position constraints in dependent coordinates. It ensures the fulfillment of mechanical holonomic constraints by keeping the state vector ${\mathbf{q}}$ on the manifold ${\boldsymbol{\Phi}}({\mathbf{q}},t)=\mathbf{0}$, hence this factor is repeated for each position node ${\mathbf{q}}_{i}$. It could be omitted for ${\mathbf{q}}_{0}$ if the enforced pose imposed by the prior factor is known to be a correct mechanism position complying with ${\boldsymbol{\Phi}}(\cdot)=\mathbf{0}$; alternatively, the prior factor can be made to affect a minimum number of variables in ${\mathbf{q}}_{0}$ (the number of degrees of freedom), leaving $f^{d}_{pc}$ in charge of recalculating the rest of the generalized coordinates. This factor, on its own, solves the so-called position problem [20] in multibody dynamics.

• 3.

  $f^{d}_{vc}$: Factor for velocity constraints in dependent coordinates, enforcing generalized velocities ${\dot{\mathbf{q}}}$ to remain on the manifold $\mathbf{\Phi_{q}}{\dot{\mathbf{q}}}+{\boldsymbol{\Phi}}_{t}=\mathbf{0}$.

• 4.

  $f^{d}_{dyn}$: Factor for the dynamics equation of motion: it links external forces (known data in this FG, hence not reflected as variable nodes) with the instantaneous acceleration ${\ddot{\mathbf{q}}}$. It also depends on ${\mathbf{q}}$ and ${\dot{\mathbf{q}}}$ since acceleration is always a function of them too. Note in the graph how acceleration for each timestep depends on position and velocity of the same timestep only.

• 5.

  $f_{ti}$: Trapezoidal numerical integrator factor is used twice per timestep: to integrate velocities into positions, and accelerations into velocities. Implicit integrators as the Trapezoidal one are often employed in MBS simulations for their stability. However, the Euler integrator version is also devised (see section 6), since explicit integrators are commonly used in real-time applications as well.

Alternatively, one could devise a FG implementation for the forward dynamics problem when using independent coordinates, leading to the graph depicted in Figure 3. In this case, the following factors are used:

• 1.

  $f_{prior}$: In this case, they are used to impose both, an initial known dynamic state (${\mathbf{z}}_{0}$, ${\dot{\mathbf{z}}}_{0}$) and approximated initial values for the full vector of dependent coordinates ${\mathbf{q}}_{0}$. The latter may be required to solve ambiguities in closed kinematic topologies, e.g. a four bar linkage, where knowing the minimum set of independent variables still leaves more than one feasible kinematic configuration.

• 2.

  $f_{eq}$: An equality factor, used to impose a soft constraint between state vectors consecutive in time. The rationale behind this factor is to impose a soft constraint, which can be easily violated (its weight is small in comparison to all other factors) but still provides a solid starting point for nonlinear numerical solvers to exploit the fact that mechanism coordinates cannot vary abruptly between consecutive time steps. This factor is particularly important to solve ambiguities in mechanisms with more than one branch, exactly as argued by the end of the former paragraph.

• 3.

  $f^{i}_{dyn}$: The independent-coordinate version of $f^{d}_{dyn}$ discussed above, it implements the dynamics equation of motion.

• 4.

  $f^{i}_{pc}$, $f^{i}_{vc}$: Like their dependent-coordinate counterparts in the former section, these factors keep the position and velocity vector within their corresponding constraint manifolds. Note how, in this case, the factors not only affect ${\mathbf{q}}$ and ${\dot{\mathbf{q}}}$ coordinates, but their independent-coordinate versions ${\mathbf{z}}$ and ${\dot{\mathbf{z}}}$ too, respectively. These factors actually correspond to the so-called position problem and velocity problem in multibody mechanics [20].

Another problem that can be formalized as a FG is inverse dynamics, as shown in Figure 4. The problem consists of specifying a desired trajectory over time for the set of degrees of freedom of a mechanism, then solving for the required forces and torques to generate such trajectory. The following factors are used:

• 1.

  $f_{prior}$: Here, different prior factors are used to define initial known values for the dynamic state (${\mathbf{q}}_{0}$, ${\dot{\mathbf{q}}}_{0}$) and to enforce a value of zero in all components of the generalized force vectors ${\mathbf{Q}}_{i}$ where it is known that no external force is acting. In other words, the latter factors are required to leave only part of ${\mathbf{Q}}_{i}$ as an unknown, e.g. for the degree of freedom where a motor is exerting a torque.

• 2.

  $f^{d}_{idyn}$: Similar to the dynamics equation factor $f^{d}_{dyn}$ discussed above, but with the force ${\mathbf{Q}}_{i}$ as an additional variable. Note that $f^{d}_{dyn}$ (see section 5.2) also used a value for ${\mathbf{Q}}_{i}$ but it was treated as a known constant, whereas for $f^{d}_{idyn}$ the ${\mathbf{Q}}_{i}$ are unknowns and as such the factor provides a Jacobian of the error term with respect to them as well.

From all discussed problems so far, inverse dynamics is the hardest to solve for nonlinear iterative solvers from initial values that are far from the optimum. Therefore, it requires a proper initialization strategy to enable numerical nonlinear solvers to cope with it effectively, as discussed in the experiments section later on.

Prior factors $f_{prior}(\mathbf{x})$ over a variable $\mathbf{x}$ are the only unary factor used in our proposed graphs. They represent a priori belief (hence the name) about the state of a given variable.

Since the problems addressed in this manuscript use variables that are either (a) state vectors with generalized coordinates of planar mechanisms, or (b) their velocities or (c) their accelerations, all variables can be treated as vectors in the group of real numbers, hence the error function $\mathbf{e}(\cdot)$ of this factor for a vector of dimensionality $d$ has a simple form:

The graphs proposed in former sections work over temporal sequences of variables, sampled at a fixed sample rate $f_{s}=1/\Delta t$. Imposing the continuity of ordinary differential equations in discrete time can be done by means of numerical integration, of which the Euler integrator is the simplest instance.

Given two consecutive samples for time steps $t$ and $t+1$ of a given variable $\mathbf{x}$ and its time derivative $\dot{\mathbf{x}}$ at time $t$, the Euler integrator factor $f_{ei}$ (shown in Figure 5(a)) can be defined as:

Another well-known numerical integration scheme follows the Trapezoidal Rule. Given two consecutive samples for time steps $t$ and $t+1$, both for a given variable $\mathbf{x}$ and its time derivative $\dot{\mathbf{x}}$, the Trapezoidal integrator factor $f_{ti}$, shown in Figure 5(b), can be defined as:

As mentioned in section 5.1, this integrator is preferred for its better accuracy in comparison to the Euler method.

This factor, depicted in Figure 5(c), ensures the fulfillment of mechanical holonomic constraints of Eq. (1). As explained in Section 2.1, modeling a mechanism in dependent coordinates leads to a number of constraint equations largest or equal to the number of generalized coordinates.

This factor has the following general form:

Velocity constraint factors further improve the quality of MBS simulation. These kind of factors are modeled by Eq. (2) and depend on both, ${\mathbf{q}}$ and ${\dot{\mathbf{q}}}$, as illustrated Figure 5(d). Its definition is as follows:

These factors minimize the error between the actual acceleration estimates (${\ddot{\mathbf{q}}}$ and ${\ddot{\mathbf{z}}}$) and the corresponding equations of motion Eq. (6) and Eq. (15) for dependent and independent coordinates, respectively. To deal both forward and inverse dynamics, this factor connects the generalized positions, velocities, and accelerations with the forces (${\mathbf{Q}}_{t}$) for each single time step $t$. In the case of forward dynamics, both the variable ${\mathbf{Q}}_{t}$ and the edge connecting it to the factor are dropped, becoming ${\mathbf{Q}}_{t}$ an internal known parameter to the factor. For this reason, two factors for forward and inverse dynamics must be defined independently:

• 1.

  Forward dynamics

  For dependent coordinates:

  $$\text{$f^{d}_{dyn}$ error function:}&\mathbf{e}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\ddot{\mathbf{q}}}_{t})={\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t})-{\ddot{\mathbf{q}}}_{t}\\ \text{Variables:}{\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\ddot{\mathbf{q}}}_{t}\\ \text{Fixed parameters:}(\text{masses, external forces})\\ \text{Jacobians:}\dfrac{\partial\mathbf{e}}{\partial{\mathbf{q}}_{t}}=\dfrac{\partial{\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t})}{\partial{\mathbf{q}}_{t}}\quad\dfrac{\partial\mathbf{e}}{\partial{\dot{\mathbf{q}}}_{t}}=\dfrac{\partial{\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t})}{\partial{\dot{\mathbf{q}}}_{t}}\quad\quad\dfrac{\partial\mathbf{e}}{\partial{\ddot{\mathbf{q}}}_{t}}=-\mathbf{I}$$

  (26)

  and for independent coordinates:

  $$\text{$f^{i}_{dyn}$ error function:}&\mathbf{e}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\ddot{\mathbf{z}}}_{t})={\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t})-{\ddot{\mathbf{z}}}_{t}\\ \text{Variables:}{\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\ddot{\mathbf{z}}}_{t}\\ \text{Fixed parameters:}(\text{masses, external forces})\\ \text{Jacobians:}\dfrac{\partial\mathbf{e}}{\partial{\mathbf{z}}_{t}}=\dfrac{\partial{\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t})}{\partial{\mathbf{z}}_{t}}\quad\dfrac{\partial\mathbf{e}}{\partial{\dot{\mathbf{z}}}_{t}}=\dfrac{\partial{\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t})}{\partial{\dot{\mathbf{z}}}_{t}}\quad\quad\dfrac{\partial\mathbf{e}}{\partial{\ddot{\mathbf{z}}}_{t}}=-\mathbf{I}$$

  (27)

• 2.

  Inverse dynamics

  For dependent coordinates:

  $$\text{$f^{d}_{dyn}$ error function:}&\mathbf{e}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\ddot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t})={\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t})-{\ddot{\mathbf{q}}}_{t}\\ \text{Variables:}{\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\ddot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t}\\ \text{Fixed parameters:}(\text{masses})\\ \text{Jacobians:}\dfrac{\partial\mathbf{e}}{\partial{\mathbf{q}}_{t}}=\dfrac{\partial{\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t})}{\partial{\mathbf{q}}_{t}}\quad\dfrac{\partial\mathbf{e}}{\partial{\dot{\mathbf{q}}}_{t}}=\dfrac{\partial{\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t})}{\partial{\dot{\mathbf{q}}}_{t}}\\ \dfrac{\partial\mathbf{e}}{\partial{\ddot{\mathbf{q}}}_{t}}=-\mathbf{I}\quad\dfrac{\partial\mathbf{e}}{\partial{\mathbf{Q}}_{t}}=\dfrac{\partial{\ddot{\mathbf{q}}}({\mathbf{q}}_{t},{\dot{\mathbf{q}}}_{t},{\mathbf{Q}}_{t})}{\partial{\mathbf{Q}}_{t}}$$

  (28)

  and for independent coordinates:

  $$\text{$f^{i}_{dyn}$ error function:}&\mathbf{e}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\ddot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t})={\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t})-{\ddot{\mathbf{z}}}_{t}\\ \text{Variables:}{\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\ddot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t}\\ \text{Fixed parameters:}(\text{masses})\\ \text{Jacobians:}\dfrac{\partial\mathbf{e}}{\partial{\mathbf{z}}_{t}}=\dfrac{\partial{\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t})}{\partial{\mathbf{z}}_{t}}\quad\dfrac{\partial\mathbf{e}}{\partial{\dot{\mathbf{z}}}_{t}}=\dfrac{\partial{\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t})}{\partial{\dot{\mathbf{z}}}_{t}}\\ \dfrac{\partial\mathbf{e}}{\partial{\ddot{\mathbf{z}}}_{t}}=-\mathbf{I}\quad\dfrac{\partial\mathbf{e}}{\partial{\mathbf{Q}}_{t}}=\dfrac{\partial{\ddot{\mathbf{z}}}({\mathbf{z}}_{t},{\dot{\mathbf{z}}}_{t},{\mathbf{Q}}_{t})}{\partial{\mathbf{Q}}_{t}}$$

  (29)

Refer to Eq. (6) and Eq. (15) for the evaluation of the error functions above. Numerical Jacobians have been used in our implementation for Eq. (28) and Eq. (29), since exact closed-form Jacobians are so complex than the computational advantage of having closed-form expressions. In this particular case is not clear and deserves future analysis.

Figure 3 shows three sets of factors linking dependent coordinates (position, velocities, and accelerations) with their corresponding degrees of freedom, that is, with their counterparts in independent coordinates. The first one of these factor is $f^{i}_{pc}$, in charge of imposing the simultaneous fulfillment of: (a) position constraints in ${\boldsymbol{\Phi}}({\mathbf{q}})$, and (b) that independent coordinates ${\mathbf{z}}$ are integrated into its corresponding positions within ${\mathbf{q}}$. Therefore, its error function and Jacobians read:

This factor ensures the fulfillment of the velocity constraints for independent coordinates:

This factor ensures the fulfillment of the acceleration constraints for independent coordinates:

Free motion of the four-bar mechanism under the sole effect of gravity ($g=9.8~{}\text{m}/\text{s}^{2}$) has been simulated, with an initial crank angle $\theta$ (refer to Figure 6(a)) of zero and null initial velocities.

The FG in Figure 2 using dependent coordinates has been implemented, and then solved numerically in an incremental fashion using a fixed-lag smoother, when only the factors of the last $N_{w}$ time steps (the window length) up to time $t$ are considered, with discrete time $t$ ranging from $0$ up to $5$ seconds with steps of $dt=1~{}\text{ms}$. A Levenberg-Marquardt nonlinear iterative solver algorithm has been selected for these runs, with a maximum number of iterations of 15. Note, however, that virtually all calls to the optimizer required only 3 to 5 iterations until convergence.

Figure 7(a)–(b) show the time history of the dependent generalized coordinates of points $P_{1}$ and $P_{2}$, and their corresponding velocities, respectively, obtained from both, the proposed FG-based approach and from MSC Adams. Both series accurately coincide, hence a more detailed comparison is in order in Figure 7(c)–(d), where the differences in ${\mathbf{q}}$ and ${\dot{\mathbf{q}}}$ are reported between the proposed FG-based method and the ground truth. Similar good agreement (omitted for brevity’s sake) is obtained when adopting the independent coordinates FG, as explained in Figure 3, thus verifying our two proposed schemes. The noise models used in all factors involved are summarized in Table 2. Covariance absolute values are not relevant, only their relative weights with respect to each other.

In order to quantify the accuracy of both schemes (dependent and independent-based FGs) and to explore the effect of the fixed-lag window length parameter $N_{w}$, the root mean square error (RMSE) of the overall ${\mathbf{q}}$ trajectories are shown in Table 1 for window size varying from $2$ to $10$ time steps. Two main conclusions can be drawn from these data: (a) the dependent-coordinates scheme seems to provide a higher accuracy, and (b) using window lengths larger than a few (3–4) time steps do not further increase the accuracy of the estimation. This latter point can be explained by the fact that the present simulations focus on forward dynamics only, not estimation from noisy sensor measurements, a case where it should be expected that larger windows lead to better results. This topic, however, falls out of the scope of the present paper and it should be studied in the future.

Next, the FG proposed in Figure 4 for inverse dynamic calculation is put at test with the same four-bar mechanism described above. Here, the crank angle $\theta$ trajectory over time is specified as an arbitrary smooth curve, and the goal is to solve for the unknown torque at the crank (point $A$) that would be required to balance gravity and inertial forces as accurately following the prescribed path.

In this case, we use a Levenberg-Marquardt nonlinear solver on the FG in Figure 4 in batch mode, as opposite to filtering or fixed-lag smoother mode. However, the position-problem is strongly nonlinear, preventing solvers to find the global optimal of the whole FG in Figure 4 for arbitrary initial values for all variables. Instead, we propose attacking the inverse dynamics FG in the following four stages (running the batch optimizer once after each stage), which ensure that the global optimum is always easy to find by the nonlinear solver:

• 1.

  Stage 1: Include ${\mathbf{q}}$ variables, prior factors for the desired trajectory ($f_{prior}$), position-constraint factors ($f^{d}_{pc}$), and soft equality factors ($f_{eq}$).

• 2.

  Stage 2: Add variables ${\dot{\mathbf{q}}}$, ${\ddot{\mathbf{q}}}$, and numerical integration factors $f_{ti}$.

• 3.

  Stage 3: Add velocity-constraint factors ($f^{d}_{vc}$).

• 4.

  Stage 4: Add generalized force variables ${\mathbf{Q}}$ and inverse dynamics factors ($f^{d}_{idyn}$).

Numerical results for this approach are shown in Figure 8, where the trajectories prescribed and actually-followed by the mechanism crank are depicted in Figure 8(a) for both the proposed approach and the commercial software MSC ADAMS. Both methods successfully solve the inverse dynamics problem by finding the torque sequence in Figure 8(b). In this case, our method leads to an excellent stable following error (less than 1 milliradian), whereas MSC’s error seems to slowly grow over time, as shown in Figure 8(c).