A general framework for modeling and dynamic simulation of multibody systems using factor graphs
AA general framework for modeling and dynamic simulationof multibody systems using factor graphs
Jos´e-Luis Blanco-Claraco ∗ , Antonio Leanza b , Giulio Reina c a Department of Engineering, University of Almer´ıa, CIESOL. Campus de Excelencia Internacional Agroalimentario, ceiA3, 04120 Almer´ıa, Spain b Department of Innovation Engineering, University of Salento, via Monteroni, 73100, Lecce, Italy c Department of Mechanics, Mathematics, and Management, Polytechnic of Bari, via Orabona 4, 70126 Bari, Italy
Abstract
In this paper, we present a novel general framework grounded in the factor graph theory to solve kinematic anddynamic problems for multi-body systems. Although the motion of multi-body systems is considered to be a well-studied problem and various methods have been proposed for its solution, a unified approach providing an intuitiveinterpretation is still pursued. We describe how to build factor graphs to model and simulate multibody systems us-ing both, independent and dependent coordinates. Then, batch optimization or a fixed-lag-smoother can be appliedto solve the underlying optimization problem that results in a highly-sparse nonlinear minimization problem. Theproposed framework has been tested in extensive simulations and validated against a commercial multibody software.We release a reference implementation as an open-source C ++ library, based on the GTSAM framework, a well-known estimation library. Simulations of forward and inverse dynamics are presented, showing comparable accuracywith classical approaches. The proposed factor graph-based framework has the potential to be integrated into appli-cations related with motion estimation and parameter identification of complex mechanical systems, ranging frommechanisms to vehicles, or robot manipulators. Keywords:
Dynamics of mechanical systems, Multibody systems, Motion state estimation, Factor graph, Nonlinearoptimization, Computational mechanics ∗ Corresponding author:
Email address: [email protected] (Jos´e-Luis Blanco-Claraco)
URL: https://w3.ual.es/personal/jlblanco/ (Jos´e-Luis Blanco-Claraco)
Preprint submitted to .. January 11, 2021 a r X i v : . [ c s . R O ] J a n ist of symbolsq ( t ) , ˙ q ( t ) , ¨ q ( t ) Vector of generalized dependent coordinates, velocities, and accelerations, respectively. z ( t ) , ˙ z ( t ) , ¨ z ( t ) Vector of generalized independent coordinates, velocities, and accelerations, respectively. Φ ( q , t ) Vector of constraint equations. Φ q Jacobian matrix of the constraints Φ . Φ t Partial derivatives of constraints Φ with respect to time t . Φ qq , ( ˙ Φ q ) q Jacobians of Φ q and ˙ Φ q with respect to q (third-order tensors). b Constraint velocities. c Constraint acceleration. λ Vector of Lagrangian multipliers. C Active ”branch” or ”configuration” of a mechanism. Q t Generalized forces for time step t . M Mass matrix for a mechanism (dependent coordinates). Q Reduced generalized forces vector. M Reduced mass matrix for a mechanism (independent coordinates). R The R matrix, mapping increments between independent and dependent velocities. n Number of dependent coordinates in q ( t ). m Number of scalar constrain equations in Φ . R The set of Real numbers.
1. Introduction
Dynamic simulation of multibody systems mainly refer to inverse and forward problems. Inverse dynamics dealswith the determination of the driving forces that generate a given motion, as well as the constraint reaction forces.The solution to the inverse dynamics problem can be obtained for example using the Newton-Euler (N-E) method[1], that results in e ffi cient recursive algorithms, e.g., Rigid Body Dynamics Library (RBDL) [2] and similar methods[3]. Conversely, forward dynamics involves the motion estimation of a multi-body system over time under the givenapplied forces and initial conditions. Therefore, in a direct dynamic problem, the motion is the result of the forcesystem that generates it. From a mathematical perspective, forward dynamics is computationally intensive as it entailsthe integration of a system of nonlinear ordinary di ff erential equations. The most common formulations that deal withforward dynamic are the Composite-Rigid-Body Algorithm (CRBA) [4] and the Articulated-Body Algorithm (ABA)[5]. However, the above mentioned methods are not suited for systems with closed-loops like parallel robots, forwhich more complex and expensive procedures are required to solve both inverse and forward dynamics, as describedfor example in [6].Therefore, a single algorithm that can solve all types of dynamics problems has not been fully established. One notablee ff ort is the work by Rodriguez [7], who proposed a unified approach based on Kalman filtering and on the conceptof smoothing to solve dynamics problems. This approach has been further extended to solve the forward dynamicproblem of closed kinematic chains [8]. Another e ff ort in defining a unifying framework has been given by [9], whoanalyzed various algorithms for serial chain dynamics. In [10], an attempt is proposed to unify the CRBA and ABAderivation as two elimination methods to solve forward dynamics.To the best of our knowledge, factor graphs have not been applied to solve the general multibody kinematics anddynamics in the existing literature. The closest works are the preprints [11, 12], where factor graphs are indeedemployed to solve kinematic and dynamic problems, although their proposed graph structure is applicable to open-loop robot arms only.The present work advocates factor graphs as a unifying graphical language in which to express the classicalkinematics and dynamics algorithms, with the additional potential to develop novel and advanced state estimators.The main contributions of this work are: • a factor graph-based representation of dynamics problems, which is a insightful visualization of the underlyingsparse constraints between all involved variables, • a unified method, which can solve inverse and forward dynamics for either open or closed kinematic chains,2 a flexible framework that can be expressed and solved for both dependent and independent coordinates.Also, note that despite the implementation described in this manuscript is focused on planar mechanisms, it isperfectly suitable to spatial systems without changes at the level of factor graphs. Additionally, our approach allowsmore powerful and flexible schemes for state and parameter estimation to be implemented in contrast with standardmethods based on Kalman filtering [13, 14, 15, 16, 17]. However, such applications are left for future extensions ofthis work to keep the present manuscript focused on the key ideas on how to apply factor graphs to multibody motionproblems.The proposed approach draws on the formalism of graphical models , a powerful tool borrowing concepts fromstatistics and graph theory [18, 19]. By addressing the multibody simulation problem from the perspective of the variable structure , graphical models allow us creating e ffi cient estimators for any combination of observed and hiddenvariables, e ff ectively unifying the problems of kinematic and forward dynamic analysis (predicting or estimating thetrajectory of a MB system), inverse dynamics, and parameter identification (e.g., inertial properties of the bodiesinvolved, external disturbance forces, friction in the joints, etc.). All those problems end up to be formulated asa sparse nonlinear cost function built from a library of reusable “building blocks” (the factors ) on which e ffi cientsolvers can then be applied. The framework is suitable for either o ffl ine batch analysis or online real-time operationthat represents another clear advantage of the proposed approach.The rest of the paper is structured as follows. Sections 2 and 3 first provide the required background on multibodydynamics and graphical models, respectively. Next, section 4 presents a methodology for the application of Bayesiannetworks to multibody dynamics problems, whereas section 5 particularizes such networks as factor graphs for anumber of practical problems. Individual factors used in those graphs are described in detail in section 6. Numericalexamples are provided in section 7, and we end sketching some conclusions in section 8.
2. Review of Multibody Dynamics
In this section, fundamentals of multibody system motion analysis are briefly recalled, whereas the interestedreader can refer for more details to the wide related Literature, e.g. [20]. A multibody system is an assembly of twoor more bodies (or elements ) constrained to each other to fulfill a given motion law. In many practical applications,these elements may be considered rigid and, throughout this paper, we will work under this assumption even thoughthe proposed framework may be further extended to include body flexibility.One of the key decisions to take when modeling a MBS is selecting the set of generalized coordinates that will beused to represent it. Using independent coordinates z allows one to deal with the lowest number of parameters, i.e.the number of DOFs of the system. A second choice is to adopt dependent coordinates q , in a number larger thanthat of DOFs but able to describe all multibody system points univocally. When dependent coordinates are used, thecorresponding set of constraint equations must be included as well for a complete system analysis. There are di ff erentkinds of dependent coordinates [20]: Relative Coordinates , References Point Coordinates , Natural Coordinates , anda combination of the previous ones (
Mixed Coordinates ). Natural coordinates, mixed with relative coordinates whereneeded, will be assumed in this work.In the remainder of this Section, the MBS motion equations are developed and expressed in terms of both depen-dent and independent coordinates.
For any given MBS with f dofs, the use of n dependent coordinates expressed by the vector q requires m = n − f constraint equations, which form the following set of equations Φ ( q ( t ) , t ) = (1)Thus, it is assumed that there are at least as many equations as there are unknown coordinates. To solve the kinematicproblem, the time derivative of Eq. (1) is required, one time for velocity analysis and two times for accelerationanalysis, leading to the following set of equations Φ q ( q ( t ) , t ) ˙ q = − Φ t ≡ b (2)3 q ( q ( t ) , t ) ¨ q = − ˙ Φ t − ˙ Φ q ˙ q ≡ c (3)where ˙ q is the vector of dependent velocities, Φ q ∈ R m × n the Jacobian matrix of Eq. (1) respect to q , and Φ t the timederivative 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’slaw, one obtains the following set of di ff erential equations that expresses the force equilibrium equations M ¨ q + Φ q (cid:62) λ = Q (4)where λ is the vector of Lagrange multipliers, M is the system mass matrix and vector Q contains the generalizedexternal forces. Since Eq. (4) is a system of n equations in n + m variables, by adding the m equations (3) one obtainsthe following system (cid:34) M Φ q (cid:62) Φ q (cid:35) (cid:34) ¨ q λ (cid:35) = (cid:34) Qc (cid:35) (5)Normally [20, 21], this equation is solved for the extended vector of unknowns that includes both, ¨ q and λ . Inour framework, we are specifically interested in the generalized accelerations ¨ q . Therefore, by applying the blockmatrix inversion lemma (see 8) to Eq. (5) and keeping the first row only, it leads to the following equation of motion expressed in terms of dependent coordinates :¨ q = (cid:16) M − − M − Φ q (cid:62) Γ − Φ q M − (cid:17) Q + (cid:16) M − Φ q (cid:62) Γ − (cid:17) c (6)where we defined the auxiliary term Γ ( q ) as the m × m square symmetric matrix Γ = Φ q M − Φ q (cid:62) , (7)introduced for convenience in subsequent derivations. Independent coordinates z , ensure the minimum number of variables, i.e. the number of DOFs. However, multi-body systems can be more di ffi cult to analyze with respect to dependent ones. First, the matrix R is introduced as˙ q = d q dt = ∂ q ∂ z ∂ z ∂ t = R ( q )˙ z (8)where the columns of R are f linearly independent vectors that constitute a basis of the nullspace of Φ q . Then, weexpress the independent velocities ˙ z as the projection of the dependent velocities ˙ q on the rows of a constant matrix B that we assume satisfying the condition of having f rows linearly independent from each other and from the m rowsof Φ q ˙ z = B ˙ q (9)By combining Eq. (2) with Eq. (9), one obtains (cid:34) Φ q B (cid:35) ˙ q = (cid:34) b ˙ z (cid:35) (10)˙ q = (cid:34) Φ q B (cid:35) − (cid:34) b ˙ z (cid:35) = Sb + R ˙ z (11)The columns of matrix R are the partial velocities with respect to the generalized coordinates z , and the term Sb represents the partial velocities with respect to time.By di ff erentiating Eq. (9) and by taking into account Eq. (3), one gets (cid:34) c ¨ z (cid:35) = (cid:34) Φ q B (cid:35) ¨ q ⇒ ¨ q = [ S R ] (cid:34) c ¨ z (cid:35) = Sc + R ¨ z (12)4rom Eq. (4), pre-multiplied by R (cid:62) R (cid:62) M ¨ q = R (cid:62) Q (13)and by substituting ¨ q in Eq. (12) R (cid:62) MR ¨ z = R (cid:62) ( Q − MSc ) (14)Introducing the reduced mass matrix M = R (cid:62) MR and force vector Q = R (cid:62) ( Q − MSc ), the following equation ofmotion in terms of independent coordinates can be finally obtained¨ z = M − Q (15)
3. Background on graphical models
A factor graph is a particular type of probabilistic graphical model that can be used to describe the structure ofan estimation problem [22]. Factor graphs have found applications in many fields, for example in robot perception[23], information theory [24], signal processing [25], and in other areas of robotics, mostly SLAM [26] and computervision [27], state estimation in legged robots [28], and kinematic motion planning [29].
It is instructive to start our discussion by considering a Dynamic Bayesian Network (DBN), a type of graphicalmodel where variables are represented as nodes and directed edges stand for causal relationships [30, 22]. Theexistence of directed edges in a DBN allows us to encode an expert’s knowledge in causality relationships betweenall 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), arediscussed in section 5. A relevant point regarding such conversion is that one single DBN can be mapped into di ff erentFGs depending on which subset of the original DBN variables are known data and which unknowns are required tobe found. Let us remark that whereas a DBN displays all variables as nodes, in a FG only unknown variables arevariable nodes. Factor graphs (FGs) are bipartite graphs comprising two kinds of nodes: variable nodes, and factor nodes. Vari-ables 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 thevariables that appear in its cost function. Sparse graph optimization algorithms exist with computational cost almostlinear with the number of edges in a FG, as opposed to, for example, the cubic cost of a naive implementation ofKalman Filtering [31]. Incremental (e.g. [32]) and sliding-window (e.g. [33]) approaches exist as well, enabling thee ffi cient 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 anumerical nonlinear minimization problem, for which very e ffi cient algorithms exist. As a probabilistic estimator,these optimal values can be assigned an uncertainty. In general, retrieving the covariance Σ of the estimated variablesinvolves inverting the Hessian of the linearized problem evaluated at the optimal solution, such that Σ = ( J (cid:62) Λ J ) − with J the sparse Jacobian of all constraints (factors), and Λ the weight matrix representing our level of certaintyabout each constraint. The matrix inversion operation is, in general, very costly since it is cubic with the problemdimension. However, optimized methods exist for the kind of sparse problem at hand, see for example, [37, §B.4] or[32].
4. DBN for multibody dynamics
Since DBNs allow a human expert to specify the causality relationships between variables, we propose the twoDBNs in Figure 1 as the underlying structure of a generic multi-body system that is the basis for our proposed frame-work. Note the use of discrete time steps t , exactly as simulations are commonly done following numerical integrationschemes. A set of observations or measurements o are available from sensors installed on the system. Note, however,5 ⋯ o Q ˙ q q ¨ q o Q ˙ q q ¨ q o Q ˙ q q ¨ q (a) Model for dependent coordinates formulation o t Q t z t ˙ z t ¨ z t ⋯ ⋯⋯ ⋯ q t ˙ q t ¨ q t o t − Q t − z t − ˙ z t − ¨ z t − q t − ˙ q t − ¨ q t − o t + Q t + z t + ˙ z t + ¨ z t + q t + ˙ q t + ¨ q t + C (b) Model for independent coordinates formulation Figure 1: Dynamic Bayesian Network (DBN) models representing the causality relationship between the variables involved in general multibodydynamics problems over discrete time steps t . Notation: q , ˙ q , ¨ q are the dependent coordinates, z , ˙ z , ¨ z are independent coordinates, o are sensorobservations, C is the active branch of the mechanism, and Q the external forces. Dashed boxes represent groups of variables for which input oroutput directed edges a ff ect all members. See section 3 for a discussion. that the graphical model formalism is flexible enough to allow each sensor to be available at a di ff erent or even sam-pling rate. When using this graphical model to achieve state estimation, sensors may provide partial information onthe system dynamics, which will be then fused with the rest of graph constraints over the system trajectory to reachthe most-likely values of the estimated variables. In non state-estimation problems, the set of observations o may beempty. Additionally, system parameters such as masses, sti ff ness or friction coe ffi cients (constant or variable overtime) could be added as additional nodes, although this possibility is left out of the scope of this manuscript for thesake of conciseness.From Figure 1, some observations can be drawn: • A central role in the DBN for independent coordinates (Figure 1(b)) is played by the independent coordinates z , ˙ z , ¨ z as they represent the degrees of freedom (d.o.f) that govern the evolution of the MBS. This role is assumedby the dependent coordinate variables q , ˙ q , ¨ q in Figure 1(a). • The branch variable C , required uniquely in the independent-coordinate formulation, allows us to disambiguatebetween, e.g. the two possible configurations for a four-bar linkage if we are only given the information aboutthe crank angle. This variable could be part of the unknowns to be estimated, as already done in [38], althoughthat work did not use the more powerful FG representation proposed here. • Typically, nodes in a DBN are depicted as shaded or unshaded depending of whether they are “hidden” or“observed” variables, respectively, e.g. see [19, 38]. The former are estimated from the latter. Since thiswork focuses on FGs instead, where observed variables are subsumed into factor nodes and unknowns become variable nodes [23], di ff erent FGs will be generated from the same DBNs in Figure 1 for di ff erent multibodysimulation problems, hence it is not convenient to establish such a distinction at the DBN level yet. • Observations o (sensor readings) are a function of (all, or a subset of) dependent coordinates. Typical observa-tions that may be useful in a MBD problem are measurements obtained from gyroscopes, accelerometers, andload cells. • External forces Q t act by means of modifying accelerations ¨ q t or ¨ z t , according to the system dynamics, ex-pressed by Eq. (6) or Eq. (15), respectively.Once the model of the dynamics MBS has been expressed as a graphical model, the time evolution of the systemcan be obtained by converting the graph into a maximum-a-posteriori (MAP) estimation problem. To this aim, one6ould write down the joint probability distribution p ( φ ) of all the involved variables φ = { q t , ˙ q t , ¨ q t , o t , Q t } for time steps 0 to t exploiting the conditional probability encoded by the DBN (refer to e.g. [19, 22]), which fordependent coordinates (i.e. Figure 1(a)) becomes: p ( φ ) = p ( q ) p ( ˙ q ) t (cid:89) i = p ( Q i ) t (cid:89) i = p ( o i | q i , ˙ q i , ¨ q i ) p ( ¨ q i | q i , ˙ q i , Q i ) p ( q i | q i − , ˙ q i − ) p ( ˙ q i | ¨ q i − , ˙ q i − ) (16)where each conditional probability term in Eq. (16), taking negative logarithms and up to an irrelevant proportionalityconstant, becomes a factor in this alternative form of an overall cost function c ( φ ) to be minimized: c ( φ ) = f prior ( q ) f prior ( ˙ q ) t (cid:89) i = f prior ( Q i ) t (cid:89) i = f obs ( o i , q i , ˙ q i , ¨ q i ) f ddyn ( ¨ q i , q i , ˙ q i , Q i ) f ei ( q i , q i − , ˙ q i − ) f ei ( ˙ q i , ¨ q i − , ˙ q i − ) (17)A fundamental feature of graphical models is how they enable factorizing functions of all problem variables suchas c ( φ ) ∝ − log p ( φ ) into the product of a large number of smaller functions (in terms of the number of variables in-volved in each expression) called factors in Eq. (17), which are discussed individually in section 6. This is what keepsthe estimation problem tractable even for hundreds of thousands of variables, something intractable for estimators inthe family of the Kalman filter not exploiting the sparsity of the problem structure. Note that the goal of an estimatoris searching for the optimal set of unknowns φ (cid:63) that maximize the likelihood of all observed data according to themodel, that is: φ (cid:63) = arg max φ p ( φ ) = arg min φ c ( φ ) (18)Depending on the division of the DBN variables into observed and unknowns, we would arrive at di ff erent factor-izations since factors are considered to be functions of the unknowns only. For example, if all variables in one givencost factor in Eq. (17) are known data it just evaluates to a constant which can be absorbed by the proportionalityrelationship c ( φ ) ∝ − log p ( φ ) and hence can be ignored. A graphical representation of the remaining relevant factorsleads us to factor graphs themselves.
5. Factor graphs for multibody dynamics
The conversion from DBN to FG is known to be achievable as follows, without the need to explicitly writing downprobabilistic equations like Eq. (16)–Eq. (17):Every Bayes net [DBN] node splits in both a variable node and a factor node in the corresponding factorgraph. The factor is connected to the variable node, as well as the variable nodes corresponding to theparent nodes in the Bayes net. If some nodes in the Bayes net are evidence nodes, i.e., they are given asknown variables, we omit the corresponding variable nodes: the known variable simply becomes a fixedparameter in the corresponding factor. [23, p. 12]Applying these rules to the DBN reflecting the structure of variables involved in the dynamic simulation of multi-body systems with dependent coordinates in Figure 1(a) we derive next the FGs for a couple of practical problemsdepending on which are the known and unknown variables. Factors will be only briefly discussed here regarding theirpurpose and meaning, whereas their detailed implementations are described in section 6.
In forward dynamics simulation, we are given a known initial state of a mechanical system (position q andvelocity q ), their geometric and inertial properties, and the external forces that act on it ( Q i , i = , ..., N ). Forsimplicity, we assume no sensors are installed in the system since this problem instance does not need them, butpredicting 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 factornodes (or plain factors ): 7 ˙ q ¨ q f vcd f dynd f pcd f ti f ti q ˙ q ¨ q f vcd f pcd f ti f ti q ˙ q ¨ q f vcd f pcd f ti f ti … … … f prior f prior f dynd f dynd Figure 2: Factor graphs for the forward dynamic simulation problem using dependent generalized coordinates. Circle nodes are problem unknowns,solid square nodes are factors. See discussion in section 5.1. • f prior : Factors imposing a given a priori knowledge about the attached variables, e.g. initial conditions. Priorfactors can be defined for both, position and velocity. • f dpc : Factor for position constraints in dependent coordinates. It ensures the fulfillment of mechanical holonomicconstraints by keeping the state vector q on the manifold Φ ( q , t ) = , hence this factor is repeated for eachposition node q i . It could be omitted for q if the enforced pose imposed by the prior factor is known to bea correct mechanism position complying with Φ ( · ) = ; alternatively, the prior factor can be made to a ff ect aminimum number of variables in q (the number of degrees of freedom), leaving f dpc in charge of recalculatingthe rest of the generalized coordinates. This factor, on its own, solves the so-called position problem [20] inmultibody dynamics. • f dvc : Factor for velocity constraints in dependent coordinates, enforcing generalized velocities ˙ q to remain onthe manifold Φ q ˙ q + Φ t = . • f ddyn : Factor for the dynamics equation of motion: it links external forces (known data in this FG, hence notreflected as variable nodes) with the instantaneous acceleration ¨ q . It also depends on q and ˙ q since accelerationis always a function of them too. Note in the graph how acceleration for each timestep depends on position andvelocity of the same timestep only. • f ti : Trapezoidal numerical integrator factor is used twice per timestep: to integrate velocities into positions, andaccelerations into velocities. Implicit integrators as the Trapezoidal one are often employed in MBS simulationsfor their stability. However, the Euler integrator version is also devised (see section 6), since explicit integratorsare commonly used in real-time applications as well. Alternatively, one could devise a FG implementation for the forward dynamics problem when using independentcoordinates , leading to the graph depicted in Figure 3. In this case, the following factors are used: • f prior : In this case, they are used to impose both, an initial known dynamic state ( z , ˙ z ) and approximated initialvalues for the full vector of dependent coordinates q . The latter may be required to solve ambiguities in closedkinematic topologies, e.g. a four bar linkage, where knowing the minimum set of independent variables stillleaves more than one feasible kinematic configuration. • f eq : An equality factor, used to impose a soft constraint between state vectors consecutive in time. The rationalebehind this factor is to impose a soft constraint , which can be easily violated (its weight is small in comparisonto all other factors) but still provides a solid starting point for nonlinear numerical solvers to exploit the factthat mechanism coordinates cannot vary abruptly between consecutive time steps. This factor is particularlyimportant to solve ambiguities in mechanisms with more than one branch, exactly as argued by the end of theformer paragraph. 8 ˙ z ¨ z f vci f dyni f pci f ti f ti … … f prior f prior q ˙ q ¨ q f prior f aci z ˙ z ¨ z f vci f dyni f pci f ti f ti q ˙ q ¨ q f aci z ˙ z ¨ z f vci f dyni f pci f ti f ti q ˙ q ¨ q f aci … f eq f eq Figure 3: Factor graph model for the forward dynamic simulation problem using independent generalized coordinates. Circle nodes are problemunknowns, solid square nodes are factors. See discussion in section 5.2. • f idyn : The independent-coordinate version of f ddyn discussed above, it implements the dynamics equation ofmotion. • f ipc , f ivc : Like their dependent-coordinate counterparts in the former section, these factors keep the position andvelocity vector within their corresponding constraint manifolds. Note how, in this case, the factors not onlya ff ect q and ˙ q coordinates, but their independent-coordinate versions z and ˙ z too, respectively. These factorsactually 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 consistsof specifying a desired trajectory over time for the set of degrees of freedom of a mechanism, then solving for therequired forces and torques to generate such trajectory. The following factors are used: • f prior : Here, di ff erent prior factors are used to define initial known values for the dynamic state ( q , ˙ q ) and toenforce a value of zero in all components of the generalized force vectors Q i where it is known that no externalforce is acting. In other words, the latter factors are required to leave only part of Q i as an unknown, e.g. forthe degree of freedom where a motor is exerting a torque. • f didyn : Similar to the dynamics equation factor f ddyn discussed above, but with the force Q i as an additionalvariable. Note that f ddyn (see section 5.2) also used a value for Q i but it was treated as a known constant, whereasfor f didyn the Q i are unknowns and as such the factor provides a Jacobian of the error term with respect to themas well.From all discussed problems so far, inverse dynamics is the hardest to solve for nonlinear iterative solvers frominitial values that are far from the optimum. Therefore, it requires a proper initialization strategy to enable numericalnonlinear solvers to cope with it e ff ectively, as discussed in the experiments section later on.
6. Factors definition
In a factor graph, factors establish the relations between variables. This section provides an insightful practicalguide to those factors MBS problems are made of. 9 ˙ q ¨ q f vcd f idynd f pcd f ti f ti q ˙ q ¨ q f vcd f pcd f ti f ti q ˙ q ¨ q f vcd f pcd f ti f ti … … … f prior f prior f idynd f idynd Q f prior Q f prior Q f prior … Figure 4: Factor graph for the inverse dynamics problem using dependent generalized coordinates. See discussion in section 5.3.
Each factor defines an error e ( x ) between predicted and measured data. To apply nonlinear optimization algo-rithms (e.g. Gauss-Newton or Levenberg-Marquardt), the Jacobian of such error with respect to all involved variables x is required. DBN variables whose values are known do not become FG nodes and hence are constant parameters of factors. The goal of the optimization is to search for the best values of all variables x (cid:63) taking the weighted sum oferror functions (one per factor) as close to zero as possible: x (cid:63) = arg min x (cid:88) i (cid:107) e i ( x ) (cid:107) Σ (19)where (cid:107) e i ( x ) (cid:107) Σ is a form of whitening already integrating the probabilistic noise model (or weight ) of each factor. Themost common model, used in the present work, is the assumption of additive zero mean Gaussian noise n ∼ N ( , Σ )with covariance matrix Σ . Taking the negative logarithm of the corresponding probability density can be shown togive us the nonlinear least-squares problem in Eq.(19), where (cid:107) e ( x ) (cid:107) Σ = e ( x ) (cid:62) Λ e ( x ) (20)with information matrix Λ = Σ − . Larger information values in Λ (or smaller variances in Σ ) imply that the factor mustbe considered with a higher weight during the optimization in comparison to other factors with smaller informationvalues (or larger variances). Note that each component of multidimensional x vectors may have a di ff erent weight fora given single factor, e.g. as proposed in the priors over external forces in section 5.3. The interested reader is deferredto [23] for a more in-depth discussion on this topic.In the following, note the use of the superscripts f d · and f i · for factors with di ff erentiated implementations fordependent and independent-coordinates, respectively. prior : Prior factor Prior factors f prior ( x ) over a variable x are the only unary factor used in our proposed graphs. They represent apriori 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 generalizedcoordinates of planar mechanisms, or (b) their velocities or (c) their accelerations, all variables can be treated asvectors in the group of real numbers, hence the error function e ( · ) of this factor for a vector of dimensionality d has asimple form: 10 t ˙ x t f ei x t + (a) Euler integration. x t ˙ x t f ti x t + ˙ x x + (b) Trapezoidal integration. q t f pc (c) Position con-straints q t ˙ q t f vc (d) Velocity constraints Figure 5: (a)–(b) Numerical integration factors discussed in sections 6.2-6.3. Factors used to enforce q and ˙ q to remain within their correspondingmanifolds: (c) holonomic position constraints, (b) velocity constraints (see sections 6.4-6.5). f prior error function: e ( x ) = x − x (21a)Variables: x (21b)Fixed parameters: x (21c)Jacobian: ∂ e ∂ x = I d (21d)where x is the “desired” value for the variable x . ei : Euler integrator factor The graphs proposed in former sections work over temporal sequences of variables, sampled at a fixed samplerate f s = / ∆ t . Imposing the continuity of ordinary di ff erential equations in discrete time can be done by means ofnumerical integration, of which the Euler integrator is the simplest instance.Given two consecutive samples for time steps t and t + x and its time derivative ˙ x at time t ,the Euler integrator factor f ei (shown in Figure 5(a)) can be defined as: f ei error function: e ( x t , x t + , ˙ x t ) = x t + − x t − ∆ t ˙ x t (22a)Variables: x t , x t + , ˙ x t (22b)Fixed parameters: ∆ t (22c)Jacobians: ∂ e ∂ x t = − I d ∂ e ∂ x t + = I d ∂ e ∂ ˙ x t = − ∆ t I d (22d)where the state space is assumed to be R d . ti : Trapezoidal integrator factor Another well-known numerical integration scheme follows the Trapezoidal Rule. Given two consecutive samplesfor time steps t and t +
1, both for a given variable x and its time derivative ˙ x , the Trapezoidal integrator factor f ti ,shown in Figure 5(b), can be defined as: f ti error function: e ( x t , x t + , ˙ x t , ˙ x t + ) = x t + − x t − ∆ t x t − ∆ t x t + (23a)Variables: x t , x t + , ˙ x t , ˙ x t + (23b)Fixed parameters: ∆ t (23c)Jacobians: ∂ e ∂ x t = − I d ∂ e ∂ x t + = I d ∂ e ∂ ˙ x t = − ∆ t I d ∂ e ∂ ˙ x t + = − ∆ t I d (23d)As mentioned in section 5.1, this integrator is preferred for its better accuracy in comparison to the Euler method.11 .4. f dpc : Factor for position constraints in dependent coordinates This factor, depicted in Figure 5(c), ensures the fulfillment of mechanical holonomic constraints of Eq. (1). Asexplained in Section 2.1, modeling a mechanism in dependent coordinates leads to a number of constraint equationslargest or equal to the number of generalized coordinates.This factor has the following general form: f dpc error function: e ( q t ) = Φ ( q t ) (24a)Variables: q t (24b)Fixed parameters: (constant distances, angles, etc.) (24c)Jacobians: ∂ e ∂ q t = Φ q ( q t ) (24d)where both Φ and Φ q can be automatically built out of pre-designed blocks (see Appendix III), according to a formaldescription of the topology of the mechanical system and the specific joints connecting each pair of adjacent bodies.In particular, since each physical constraint becomes one or more entries in the Φ ( q ) vector, each such constraintfully determines the corresponding rows in the Jacobian Φ q ( q ), easing its automated construction from the elementalexpressions exposed in Appendix III. dvc : Factor for velocity constraints in dependent coordinates Velocity constraint factors further improve the quality of MBS simulation. These kind of factors are modeled byEq. (2) and depend on both, q and ˙ q , as illustrated Figure 5(d). Its definition is as follows: f dvc error function: e ( q t , ˙ q t ) = Φ q ( q t ) ˙ q t − (cid:24)(cid:24)(cid:24)(cid:24)(cid:58) Φ t ( q t ) = Φ q ( q t ) ˙ q t (25a)Variables: q t , ˙ q t (25b)Fixed parameters: (constant distances, angles, etc.) (25c)Jacobians: ∂ e ∂ q t = ∂ Φ q ( q t ) ˙ q t ∂ q t ∂ e ∂ ˙ q t = Φ q ( q t ) (25d)where we assumed that no constraint depends explicitly on time, hence Φ t ( q t ) can be safely neglected. Again, eachrow in the Jacobians comes from exactly one constraint in the definition of the mechanical system, with most commoncases described in Appendix III. ddyn , f didyn and f idyn , f iidyn : Factors for dynamics These factors minimize the error between the actual acceleration estimates ( ¨ q and ¨ z ) and the corresponding equa-tions of motion Eq. (6) and Eq. (15) for dependent and independent coordinates, respectively. To deal both forwardand inverse dynamics, this factor connects the generalized positions, velocities, and accelerations with the forces ( Q t )for each single time step t . In the case of forward dynamics, both the variable Q t and the edge connecting it to thefactor are dropped, becoming Q t an internal known parameter to the factor. For this reason, two factors for forwardand inverse dynamics must be defined independently: • Forward dynamics
For dependent coordinates: f ddyn error function: e ( q t , ˙ q t , ¨ q t ) = ¨ q ( q t , ˙ q t ) − ¨ q t (26a)Variables: q t , ˙ q t , ¨ q t (26b)Fixed parameters: (masses, external forces) (26c)Jacobians: ∂ e ∂ q t = ∂ ¨ q ( q t , ˙ q t ) ∂ q t ∂ e ∂ ˙ q t = ∂ ¨ q ( q t , ˙ q t ) ∂ ˙ q t ∂ e ∂ ¨ q t = − I (26d)12nd for independent coordinates: f idyn error function: e ( z t , ˙ z t , ¨ z t ) = ¨ z ( z t , ˙ z t ) − ¨ z t (27a)Variables: z t , ˙ z t , ¨ z t (27b)Fixed parameters: (masses, external forces) (27c)Jacobians: ∂ e ∂ z t = ∂ ¨ z ( z t , ˙ z t ) ∂ z t ∂ e ∂ ˙ z t = ∂ ¨ z ( z t , ˙ z t ) ∂ ˙ z t ∂ e ∂ ¨ z t = − I (27d) • Inverse dynamics
For dependent coordinates: f ddyn error function: e ( q t , ˙ q t , ¨ q t , Q t ) = ¨ q ( q t , ˙ q t , Q t ) − ¨ q t (28a)Variables: q t , ˙ q t , ¨ q t , Q t (28b)Fixed parameters: (masses) (28c)Jacobians: ∂ e ∂ q t = ∂ ¨ q ( q t , ˙ q t , Q t ) ∂ q t ∂ e ∂ ˙ q t = ∂ ¨ q ( q t , ˙ q t , Q t ) ∂ ˙ q t ∂ e ∂ ¨ q t = − I ∂ e ∂ Q t = ∂ ¨ q ( q t , ˙ q t , Q t ) ∂ Q t (28d)and for independent coordinates: f idyn error function: e ( z t , ˙ z t , ¨ z t , Q t ) = ¨ z ( z t , ˙ z t , Q t ) − ¨ z t (29a)Variables: z t , ˙ z t , ¨ z t , Q t (29b)Fixed parameters: (masses) (29c)Jacobians: ∂ e ∂ z t = ∂ ¨ z ( z t , ˙ z t , Q t ) ∂ z t ∂ e ∂ ˙ z t = ∂ ¨ z ( z t , ˙ z t , Q t ) ∂ ˙ z t ∂ e ∂ ¨ z t = − I ∂ e ∂ Q t = ∂ ¨ z ( z t , ˙ z t , Q t ) ∂ Q t (29d)Refer to Eq. (6) and Eq. (15) for the evaluation of the error functions above. Numerical Jacobians have beenused in our implementation for Eq. (28d) and Eq. (29d), since exact closed-form Jacobians are so complex than thecomputational advantage of having closed-form expressions. In this particular case is not clear and deserves futureanalysis. ipc : Factor for position constraints in independent coordinates Figure 3 shows three sets of factors linking dependent coordinates (position, velocities, and accelerations) withtheir corresponding degrees of freedom, that is, with their counterparts in independent coordinates. The first one ofthese factor is f ipc , in charge of imposing the simultaneous fulfillment of: (a) position constraints in Φ ( q ), and (b) thatindependent coordinates z are integrated into its corresponding positions within q . Therefore, its error function andJacobians read: f ipc error function: e ( q t , z t ) = (cid:34) Φ ( q t ) q t ( { idxs } ) − z t (cid:35) ( m + d ) × (30a)Variables: q t , z t (30b)Fixed parameters: { idxs } , constant distances, angles, etc. (30c)Jacobians: ∂ e ∂ q t = (cid:34) Φ q ( q t ) I idxs (cid:35) ( m + d ) × n ∂ e ∂ z t = (cid:34) m × d − I d (cid:35) ( m + d ) × d (30d)13here { idxs } = { y , y , ..., y d } stands for the fixed sequence of indices of each z coordinate inside the n -vector q , andthe coe ffi cients I i , j of the binary matrix I idxs are defined as 1 if j = y i , or as 0 otherwise, where i = , ..., d and j = , ..., n . ivc : Factor for velocity constraints in independent coordinates This factor ensures the fulfillment of the velocity constraints for independent coordinates: f ivc error function: e ( q t , ˙ q t , ˙ z t ) = (cid:34) Φ q ( q t ) ˙ q t ˙ q t ( { idxs } ) − ˙ z t (cid:35) ( m + d ) × (31a)Variables: q t , ˙ q t , ˙ z t (31b)Fixed parameters: { idxs } , constant distances, angles, etc. (31c)Jacobians: ∂ e ∂ q t = (cid:34) Φ qq ( q t ) ˙ q t d × n (cid:35) ( m + d ) × n ∂ e ∂ ˙ q t = (cid:34) Φ q ( q t ) I idx (cid:35) ( m + d ) × n ∂ e ∂ ˙ z t = (cid:34) m × d − I d (cid:35) ( m + d ) × d (31d)where the tensor-vector product Φ qq ˙ q t results in the 2-D matrix ˙ Φ q (see Eq. (40a)) which can be systematically builtrow by row from a description of the mechanism from the building blocks described in Appendix III. iac : Factor for acceleration constraints in independent coordinates This factor ensures the fulfillment of the acceleration constraints for independent coordinates: f ivc error function: e ( q t , ˙ q t , ¨ q t , ¨ z t ) = (cid:34) ˙ Φ q ( q t ) ˙ q t + Φ q ( q t ) ¨ q t ¨ q t ( { idxs } ) − ¨ z t (cid:35) ( m + d ) × (32a)Variables: q t , ˙ q t , ¨ q t , ¨ z t (32b)Fixed parameters: (constant distances, angles, etc.) (32c)Jacobians: ∂ e ∂ q t = (cid:34) ( ˙ Φ q ) q ( q t ) ˙ q t + Φ qq ( q t ) ¨ q t d × n (cid:35) ( m + d ) × n ∂ e ∂ ˙ q t = (cid:34) Φ qq ( q t ) ˙ q t d × n (cid:35) ( m + d ) × n ∂ e ∂ ¨ q t = (cid:34) Φ q ( q t ) I idx (cid:35) ( m + d ) × n ∂ e ∂ ¨ z t = (cid:34) m × d − I d (cid:35) ( m + d ) × d (32d)where the tensor-vector products ( ˙ Φ q ) q ( q t ) ˙ q t , Φ qq ¨ q t , and 2 Φ qq ˙ q t = Φ q can be systematically built as described inAppendix III.
7. Test case validation
A four-bar linkage is employed to exemplify the implementation and the performance of the proposed FG-basedframework. A scheme of the mechanism is shown in Figure 6(a), where the two revolute joints, P and P , canmove in the motion plane, whereas A and D are fixed. Geometric and inertial properties are summarized in Fig-ure 6(b). The motion of the mechanism has been simulated using the commercial MBS environment Adams / Viewfrom
MSC.Software with a fixed-step integrator at 1 /
10 of the time step used in the factor graph, hence its results canbe considered the ground-truth for comparison purposes.A companion open-source reference C ++ implementation of the proposed factor graph approach has been re-leased along with the paper. Note that all factor closed-form Jacobians have been thoroughly validated by means ofunit tests against numerical finite di ff erences. In https://github.com/MBDS/multibody-state-estimation P : ( x , y ) P : ( x , y ) DA (a) L L L √
13 [m] m m m I i , g m i L i [kg m ] (b) Figure 6: (a) The planar mechanism used in the numeric simulations described in section 7. (b) Table of mechanism properties (length and mass)for each link. Inertias I i are given with respect to the center of mass of each link, placed at its center. Free motion of the four-bar mechanism under the sole e ff ect of gravity ( g = . / s ) has been simulated, withan initial crank angle θ (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 incre-mental fashion using a fixed-lag smoother, when only the factors of the last N w time steps (the window length ) up totime t are considered, with discrete time t ranging from 0 up to 5 seconds with steps of dt = P and P , and theircorresponding 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 di ff erencesin q and ˙ q are reported between the proposed FG-based method and the ground truth. Similar good agreement (omittedfor brevity’s sake) is obtained when adopting the independent coordinates FG, as explained in Figure 3, thus verifyingour two proposed schemes. The noise models used in all factors involved are summarized in Table 2. Covarianceabsolute 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 thee ff ect of the fixed-lag window length parameter N w , the root mean square error (RMSE) of the overall q trajectoriesare shown in Table 1 for window size varying from 2 to 10 time steps. Two main conclusions can be drawn from thesedata: (a) the dependent-coordinates scheme seems to provide a higher accuracy, and (b) using window lengths largerthan a few (3–4) time steps do not further increase the accuracy of the estimation. This latter point can be explained bythe 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 thescope of the present paper and it should be studied in the future. N w DC FG IC FG( timesteps ) RMSE (m) RMSE (m)2 0.002361 0.0238343 0.002356 0.0196454 0.002352 0.0200195 0.002348 0.0166476 0.002344 0.0166777 0.002341 0.0166778 0.002338 0.0166779 0.002335 0.01705610 0.002332 0.016677
Table 1: RMSE for q trajectories estimated by the dependent coordinates (DC) and independent coordinates (IC) factor graphs (FG) for thesimulation described in the text, with respect to ground-truth, for di ff erent window lengths of the fixed-lag smoother estimator. time [s] -101 [ m ] x1 MSC.ADAMSOurs time [s] -1-0.500.5 [ m ] y1 MSC.ADAMSOurs time [s] [ m ] x2 MSC.ADAMSOurs time [s] [ m ] y2 MSC.ADAMSOurs (a) Comparison of q series. time [s] -10010 [ m / s ] dx1 MSC.ADAMSOurs time [s] -505 [ m / s ] dy1 MSC.ADAMSOurs time [s] -202 [ m / s ] dx2 MSC.ADAMSOurs time [s] -505 [ m / s ] dy2 MSC.ADAMSOurs (b) Comparison of ˙ q series. time [s] -0.015-0.01-0.00500.0050.010.015 E rr o r i n q [ m ] x1y1x2y2 (c) Error in q . time [s] -0.15-0.1-0.0500.050.1 E rr o r i n dq [ m / s ] dx1dy1dx2dy2 (d) Error in ˙ q . Figure 7: Simulation-based validation of the proposed FG approach to forward dynamics for a four-bar linkage under the e ff ects of gravity.Generalized coordinates (a) and velocities (b) are shown for our approach (using a fixed-lag smoother with N w =
2, time step of 1 ms) and forMSC.ADAMS as ground truth. (c) and (d) show the error as the di ff erence between ground truth and the output of the proposed FG-based method.Overall RMSE for this case are ≈ . ≈ .
021 m / s for q and ˙ q , respectively. See discussion in section 7.1. Factor Noise model f prior ( q ) n × n f prior ( q ) diag ( { − if q ( i ) ∈ z , } f ti − I n f ddyn , f idyn − I n f dpc , f dvc m × m f ipc , f ivc ( m + d ) × ( m + d ) f eq I n Table 2: Noise models (covariance matrices) employed in the forward dynamics simulations. Most covariances represent isotropic models (thoseincluding the identity matrix I , or for perfectly-known values), with only a few of them having di ff erent weights for di ff erent coordinates (thosedefined with a diag ( · · · ), a diagonal matrix with the given diagonal values). Next, the FG proposed in Figure 4 for inverse dynamic calculation is put at test with the same four-bar mechanismdescribed above. Here, the crank angle θ trajectory over time is specified as an arbitrary smooth curve, and the goal isto solve for the unknown torque at the crank (point A ) that would be required to balance gravity and inertial forces asaccurately following the prescribed path.In this case, we use a Levenberg-Marquardt nonlinear solver on the FG in Figure 4 in batch mode, as oppositeto filtering or fixed-lag smoother mode. However, the position-problem is strongly nonlinear, preventing solvers tofind the global optimal of the whole FG in Figure 4 for arbitrary initial values for all variables. Instead, we proposeattacking 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:16 Stage 1: Include q variables, prior factors for the desired trajectory ( f prior ), position-constraint factors ( f dpc ), andsoft equality factors ( f eq ). • Stage 2: Add variables ˙ q , ¨ q , and numerical integration factors f ti . • Stage 3: Add velocity-constraint factors ( f dvc ). • Stage 4: Add generalized force variables Q and inverse dynamics factors ( f didyn ).Numerical results for this approach are shown in Figure 8, where the trajectories prescribed and actually-followedby the mechanism crank are depicted in Figure 8(a) for both the proposed approach and the commercial softwareMSC ADAMS. Both methods successfully solve the inverse dynamics problem by finding the torque sequence inFigure 8(b). In this case, our method leads to an excellent stable following error (less than 1 milliradian), whereasMSC’s error seems to slowly grow over time, as shown in Figure 8(c).
8. Conclusions and future works
This work has settled the theoretical bases for formulating kinematics and dynamics problems from computationalmechanics in the form of factor graphs, and demonstrated the validity of our reference open-source implementation.The results showed the practical feasibility of the proposed approach and its accuracy in comparison with a commercialMBS software based on classical approaches. One additional advantage of our formalism lies at its excellent suitabilityto design state observers and parameter identification systems for arbitrary mechanical systems, ranging from robotsto vehicles. This aspect will be part of future works.
Appendix I. Block matrix inversion lemma
This lemma implies that: (cid:34)
A BC D (cid:35) − = A − + A − B (cid:16) D − CA − B (cid:17) − CA − − A − B (cid:16) D − CA − B (cid:17) − − (cid:16) D − CA − B (cid:17) − CA − (cid:16) D − CA − B (cid:17) − (33) Appendix II. Tensor notation definitions and useful expressions
Order of a tensor : Number of indices to index all its components, i.e. Scalars, vectors, and matrices are 0th, 1st,and 2nd-order tensors, respectively.
Einstein convention : Superscript indices imply reading elements up-down, subscript indices imply reading left-right. Summation works over the repeated index or indices. Example with a common matrix product: C = AB ⇒ (cid:40) Conventional notation: C ik = (cid:80) j A i j B jk Einstein convention: C ik = A i j B jk (34) Transpose rule : We have that A i j = ( A (cid:62) ) ji . Jacobian : ∂ x ∂ y , assuming x and y are a row and a column vector, respectively, becomes: ∂ x ∂ y = ∂ x i ∂ y j = ∆ i j (35) The standard derivative chain rule : Applied to vector variables x and z , using an intermediary variable y , can beput in tensor notation as: ∂ x ∂ y = ∂ x i ∂ z k (cid:12)(cid:12)(cid:12)(cid:12)(cid:12) n i × n k = ∂ x i ∂ y j ∂ y j ∂ z k = ∆ i j (cid:124)(cid:123)(cid:122)(cid:125) n i × n j ∆ jk (cid:124)(cid:123)(cid:122)(cid:125) n j × n k ≡ (cid:88) j ∂ x i ∂ y j ∂ y j ∂ z k (36)17 t [s] -0.8-0.6-0.4-0.200.20.40.60.8 [ r ad ] FGMSCReference (a) Crank angle reference and simulated trajectories. t [s] -150-100-50050100150200 T o r que [ N m ] FGMSC (b) Computed actuator torque. t [s] -6-4-202468 - r e f [ r ad ] -3 FGMSC (c) Tracking error of the crank angle.
Figure 8: Evaluation of the inverse dynamics test with a four-bar mechanism controlled with a torque at the input crank. Refer to discussion insection 7.2. hain rule for derivatives of matrices with respect to a vector : It gives us the third-order tensor: ∂ A ∂ z = ∂ A i j ∂ z k (cid:124)(cid:123)(cid:122)(cid:125) n i × n j × n k = ∂ A i j ∂ y l (cid:124)(cid:123)(cid:122)(cid:125) n i × n j × n l ∂ y l ∂ z k (cid:124)(cid:123)(cid:122)(cid:125) n l × n k (37)where it becomes clear that we must sum over the l indices, that is, converting the Einstein notation above into aconventional summation: ∂ A ∂ z = (cid:88) l ∂ A ∂ y l (cid:124)(cid:123)(cid:122)(cid:125) n i × n j × n l ∂ y l ∂ z (cid:124)(cid:123)(cid:122)(cid:125) n l × n k (38) Product of a third-order tensor and a vector : Given a third-order tensor T = T ij k and a vector v = v k , its productreads: Tv = T ij k v k = (cid:88) k T ijk v k (39) Appendix III. Equations for planar multibody systems
In the following, we summarize the equations implemented in our open-source library to model typical mechan-ical constraints of planar mechanisms. These equations are the building blocks of the errors and Jacobian matricesassociated with the factors described in sections 6.4-6.5. An open-source implementation of all these equations isprovided in the C ++ toolkit mentioned in section 7.Since each constraint contributes only a few scalar components to Φ (typically only one for planar mechanisms),the following uses the superscript Φ k to reflect the k -th row in the corresponding vector or matrix, with k beingarbitrarily determined by the order in which constraints are enumerated when defining a mechanism. Furthermore,for the sake of notation clarity, q should be understood in the following sets of equations as the subset of the actual q that contains only those generalized coordinates that are directly involved in the constraint, i.e. the provided Jacobiansshould be expanded with columns of zeros as needed at the position of non-relevant coordinates.In addition to the specific formulas for each constraint, the following terms appear in factor Jacobians but aregeneric so they apply to all constraints:˙ Φ k q ( q ) = d Φ q k ( q ) dt = ∂ Φ q k ∂ q ˙ q + (cid:26)(cid:26)(cid:62) Φ t = Φ qq k ˙ q (40a) ∂ ˙ Φ k q ( q ) ∂ ˙ q = Φ qq k (From Eq. (40a)) (40b) ∂ (cid:16) Φ q k ( q ) ˙ q (cid:17) ∂ q = Φ qq k ˙ q = ˙ Φ k q ( q ) (40c) ∂ (cid:16) Φ q k ( q ) ˙ q (cid:17) ∂ ˙ q = Φ q k (40d) Constant distance
This constraint is typically used to model rigid solids. Given the coordinates of any pair of points i and j of arigid body, such that ( x i , y i ) ∈ q and ( x j , y j ) ∈ q , this constraint ensures a fixed distance L between them. Each suchconstraint contributes with the following rows in the di ff erent vectors and matrices defining the multibody system:19 k ( q ) = (cid:16) x j − x i (cid:17) + (cid:16) y j − y i (cid:17) − L = Φ q k ( q ) = (cid:104) (cid:16) x i − x j (cid:17) (cid:16) y i − y j (cid:17) (cid:16) x j − x i (cid:17) (cid:16) y j − y i (cid:17)(cid:105) (41b)˙ Φ k q ( q ) = (cid:104) (cid:16) ˙ x i − ˙ x j (cid:17) (cid:16) ˙ y i − ˙ y j (cid:17) (cid:16) ˙ x j − ˙ x i (cid:17) (cid:16) ˙ y j − ˙ y i (cid:17)(cid:105) (41c) Φ qq k ( q ) = ∂ Φ q k ∂ q = ∂ Φ q k ∂ x ∂ Φ q k ∂ y ∂ Φ q k ∂ x ∂ Φ q k ∂ y = x i y i x j y j − Φ q k − Φ q k − Φ q k − Φ q k (41d)( ˙ Φ q ) k q ( q ) = × (41e) Φ qq k ( q ) ¨ q = (cid:104) (cid:16) ¨ x i − ¨ x j (cid:17) (cid:16) ¨ y i − ¨ y j (cid:17) (cid:16) ¨ x j − ¨ x i (cid:17) (cid:16) ¨ y j − ¨ y i (cid:17)(cid:105) (41f) Fixed pinned slider constraint
This constraints model a point P ( x , y ) enforced to move along a line defined by two fixed points A ( x A , y A ) and B ( x B , y B ). By exploiting the properties of the similar triangles, one obtains: Φ k ( q ) = ( x B − x A ) ( y − y A ) − ( y B − y A ) ( x − x A ) (42a) Φ q k ( q ) = (cid:2) − ( y B − y A ) ( x B − x A ) (cid:3) (42b)˙ Φ k q ( q ) = × (42c) Φ qq k ( q ) = × (42d) Mobile pinned slider constraint
A more general version of the former constraint, where the point P( x , y ) is a now moving along a line defined bytwo mobile points A( x i , y i ) and B( x j , y j ). In this case: Φ k ( q ) = ( x j − x i )( y − y i ) − ( y j − y i )( x − x i ) (43a) Φ q k ( q ) = (cid:104) y i − y j x j − x i y j − y x − x j y − y i x i − x (cid:105) (43b)˙ Φ k q ( q ) = (cid:104) ˙ y i − ˙ y j ˙ x j − ˙ x i ˙ y j − ˙ y ˙ x − ˙ x j ˙ y − ˙ y i ˙ x i − ˙ x (cid:105) (43c) Φ qq k ( q ) = x y x i y i x j y j − Φ q k − Φ q k − Φ q k − Φ q k − Φ q k − Φ q k (43d)( ˙ Φ q ) k q ( q ) = × (43e) Φ qq k ( q ) ¨ q = (cid:104) ¨ y i − ¨ y j ¨ x j − ¨ x i ¨ y j − ¨ y ¨ x − ¨ x j ¨ y − ¨ y i ¨ x i − ¨ x (cid:105) (43f)20 bsolute orientation of planar link Another special kind of constraints is that one needed when defining relative coordinates, such as the absoluteangle of a link, e.g. a mechanism input crank, or a vehicle wheel position. The relative angle discussed in this sectionis called absolute since it is defined as the angle θ between a body comprising a pair of points ( x i , y i )–( x j , y j ), at afixed distance L , and the horizontal axis of the global frame of reference.For this constraint, with the ordering of relevant coordinates being q = [ x i y i x j y j θ ] (cid:62) , two sets of equations existto avoid degeneracy. The first ones are used when | sin( θ ) | > / √ Φ k ( q ) = x j − x i − L cos( θ ) = Φ q k ( q ) = [ − L sin( θ )] (44b)˙ Φ k q ( q ) = (cid:104) L ˙ θ cos( θ ) (cid:105) (44c) Φ qq k ( q ) = x i y i x j y j θ Φ q k Φ q k Φ q k Φ q k L cos ( θ ) Φ q k (44d)( ˙ Φ q ) k q ( q ) = x i y i x j y j θ Φ k q Φ k q Φ k q Φ k q − L ˙ θ sin ( θ ) ˙ Φ k q (44e) Φ qq k ( q ) ¨ q = (cid:104) L ¨ θ cos ( θ ) (cid:105) (44f)( ˙ Φ q ) k q ( q ) ˙ q = (cid:104) − L ˙ θ sin ( θ ) (cid:105) (44g)and the following alternative ones are used otherwise: 21 k ( q ) = y j − y i − L sin( θ ) = Φ q k ( q ) = [0 − − L cos( θ )] (45b)˙ Φ k q ( q ) = (cid:104) L ˙ θ sin( θ ) (cid:105) (45c) Φ qq k ( q ) = x i y i x j y j θ Φ q k Φ q k Φ q k Φ q k L sin ( θ ) Φ q k (45d)( ˙ Φ q ) k q ( q ) = x i y i x j y j θ Φ k q Φ k q Φ k q Φ k q L ˙ θ cos ( θ ) ˙ Φ k q (45e) Φ qq k ( q ) ¨ q = (cid:104) L ¨ θ sin ( θ ) (cid:105) (45f)( ˙ Φ q ) k q ( q ) ˙ q = (cid:104) L ˙ θ cos ( θ ) (cid:105) (45g) References [1] B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics, Springer, 2009.[2] M. L. Felis, Rbdl: An e ffi cient rigid-body dynamics library using recursive algorithms, Autonomous Robots 41 (2) (2017) 495–511.[3] D. E. Orin, R. McGhee, M. Vukobratovi, , G. Hartoch, Kinematic and kinetic analysis of open-chain linkages utilizing newton-euler methods,Mathematical Biosciences 43 (1-2) (1979) 107–130.[4] M. W. Walker, D. E. Orin, E ffi cient Dynamic Computer Simulation of Robotic Mechanisms, Journal of Dynamic Systems, Measurement,and Control 104 (3) (1982) 205–211, publisher: American Society of Mechanical Engineers Digital Collection. doi:10.1115/1.3139699 .URL https://asmedigitalcollection.asme.org/dynamicsystems/article/104/3/205/428542/Efficient-Dynamic-Computer-Simulation-of-Robotic [5] R. Featherstone, D. Orin, Robot dynamics: equations and algorithms, in: Proceedings 2000 ICRA. Millennium Conference. IEEE Inter-national Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), Vol. 1, 2000, pp. 826–834 vol.1, iSSN:1050-4729. doi:10.1109/ROBOT.2000.844153 .[6] J. Wang, G. C.M., A new approach for the dynamic analysis of parallel manipulators, A new approach for the dynamic analysis of parallelmanipulators 2 (3) (1998) 317–334.[7] G. Rodriguez, Kalman filtering, smoothing, and recursive robot arm forward and inverse dynamics, IEEE Journal on Robotics and Automation3 (6) (1987) 624–639.[8] G. Rodriguez, Recursive forward dynamics for multiple robot arms moving a common task object, IEEE Transactions on Robotics andAutomation 5 (4) (1989) 510–521.[9] A. Jain, Unified formulation of dynamics for serial rigid multibody systems, Journal of Guidance, Control, and Dynamics 14 (3) (1991)531–542.[10] U. M. Ascher, P. K. Dinesh, B. P. Cloutier, Forward dynamics, elimination methods, and formulation sti ff ness in robot simulation, TheInternational Journal of Robotics Research 16 (6) (1997) 749–758.[11] M. Xie, F. Dellaert, Batch and Incremental Kinodynamic Motion Planning using Dynamic Factor Graphs, arXiv:2005.12514 [cs]ArXiv:2005.12514.URL http://arxiv.org/abs/2005.12514 [12] M. Xie, F. Dellaert, A Unified Method for Solving Inverse, Forward, and Hybrid Manipulator Dynamics using Factor Graphs,arXiv:1911.10065 [cs]ArXiv: 1911.10065.URL http://arxiv.org/abs/1911.10065 [13] J. Cuadrado, D. Dopico, A. Barreiro, E. Delgado, Real-time state observers based on multibody models and the extended kalman filter,Journal of mechanical science and technology 23 (4) (2009) 894–900.[14] R. Pastorino, D. Richiedei, J. Cuadrado, A. Trevisani, State estimation using multibody models and nonlinear kalman filters, InternationalJournal of Non-Linear Mechanics.
15] F. Naets, R. Pastorino, J. Cuadrado, W. Desmet, Online state and input force estimation for multibody models employing extended kalmanfiltering, Multibody System Dynamics 32 (3) (2014) 317–336.[16] E. Sanjurjo, M. ´A. Naya, J. L. Blanco-Claraco, J. L. Torres-Moreno, A. Gim´enez-Fern´andez, Accuracy and e ffi ciency comparison of variousnonlinear kalman filters applied to multibody models, Nonlinear Dynamics 88 (3) (2017) 1935–1951.[17] G. Reina, M. Paiano, J. Blanco-Claraco, Vehicle parameter estimation using a model-based estimator, Mechanical Systems and Signal Pro-cessing 87 (4) (2017) 227–241.[18] H.-A. Loeliger, An introduction to factor graphs, IEEE Signal Processing Magazine 21 (1) (2004) 28–41.[19] C. Bishop, Pattern recognition and machine learning, Springer New York, 2006.[20] J. Garc´ıa de Jal´on, E. Bayo, Kinematic and dynamic simulation of multibody systems: The real time challenge, Springer-Verlag, 1994.[21] A. A. Shabana, Dynamics of multibody systems, Cambridge university press, 2005.[22] D. Koller, N. Friedman, Probabilistic graphical models: principles and techniques, MIT press, 2009.[23] F. Dellaert, M. Kaess, Factor graphs for robot perception, Foundations and Trends ® in Robotics 6 (1-2) (2017) 1–139.[24] A. P. Worthen, W. E. Stark, Unified design of iterative receivers using factor graphs, IEEE Transactions on Information Theory 47 (2) (2001)843–849.[25] H.-A. Loeliger, J. Dauwels, J. Hu, S. Korl, L. Ping, F. R. Kschischang, The factor graph approach to model-based signal processing, Proceed-ings of the IEEE 95 (6) (2007) 1295–1322.[26] G. Grisetti, R. K¨ummerle, C. Stachniss, W. Burgard, A tutorial on graph-based slam, IEEE Intelligent Transportation Systems Magazine 2 (4)(2010) 31–43.[27] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, Imu preintegration on manifold for e ffi cient visual-inertial maximum-a-posteriori estima-tion, in: Proceedings of Robotics: Science and Systems, Rome, Italy, 2015. doi:10.15607/RSS.2015.XI.006 .[28] R. Hartley, J. Mangelson, L. Gan, M. G. Jadidi, J. M. Walls, R. M. Eustice, J. W. Grizzle, Legged robot state-estimation through combinedforward kinematic and preintegrated contact factors, in: 2018 IEEE International Conference on Robotics and Automation (ICRA), IEEE,2018, pp. 1–8.[29] I. Sugiarto, J. Conradt, Discrete belief propagation network using population coding and factor graph for kinematic control of a mobile robot,in: 2013 IEEE International Conference on Computational Intelligence and Cybernetics (CYBERNETICSCOM), IEEE, 2013, pp. 136–140.[30] F. V. Jensen, An introduction to Bayesian networks, Vol. 74, UCL press London, 1996.[31] M. Raitoharju, R. Pich´e, On computational complexity reduction methods for kalman filter extensions, IEEE Aerospace and ElectronicSystems Magazine 34 (10) (2019) 2–19.[32] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, F. Dellaert, isam2: Incremental smoothing and mapping using the bayes tree, TheInternational Journal of Robotics Research 31 (2) (2012) 216–235.[33] H. Strasdat, A. J. Davison, J. M. Montiel, K. Konolige, Double window optimisation for constant time visual slam, in: 2011 internationalconference on computer vision, IEEE, 2011, pp. 2352–2359.[34] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, Keyframe-based visual–inertial odometry using nonlinear optimization, TheInternational Journal of Robotics Research 34 (3) (2015) 314–334.[35] T. Qin, P. Li, S. Shen, Vins-mono: A robust and versatile monocular visual-inertial state estimator, IEEE Transactions on Robotics 34 (4)(2018) 1004–1020.[36] S. Agarwal, Y. Furukawa, N. Snavely, I. Simon, B. Curless, S. M. Seitz, R. Szeliski, Building rome in a day, Communications of the ACM54 (10) (2011) 105–112.[37] B. Triggs, P. F. McLauchlan, R. I. Hartley, A. W. Fitzgibbon, Bundle adjustment—a modern synthesis, in: Vision algorithms: theory andpractice, Springer, 2000, pp. 298–372.[38] J.-L. Blanco, J.-L. Torres-Moreno, A. Gim´enez-Fern´andez, Multibody dynamic systems as bayesian networks: applications to robust stateestimation of mechanisms, Multibody System Dynamics 34 (2015) 103–128..[28] R. Hartley, J. Mangelson, L. Gan, M. G. Jadidi, J. M. Walls, R. M. Eustice, J. W. Grizzle, Legged robot state-estimation through combinedforward kinematic and preintegrated contact factors, in: 2018 IEEE International Conference on Robotics and Automation (ICRA), IEEE,2018, pp. 1–8.[29] I. Sugiarto, J. Conradt, Discrete belief propagation network using population coding and factor graph for kinematic control of a mobile robot,in: 2013 IEEE International Conference on Computational Intelligence and Cybernetics (CYBERNETICSCOM), IEEE, 2013, pp. 136–140.[30] F. V. Jensen, An introduction to Bayesian networks, Vol. 74, UCL press London, 1996.[31] M. Raitoharju, R. Pich´e, On computational complexity reduction methods for kalman filter extensions, IEEE Aerospace and ElectronicSystems Magazine 34 (10) (2019) 2–19.[32] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, F. Dellaert, isam2: Incremental smoothing and mapping using the bayes tree, TheInternational Journal of Robotics Research 31 (2) (2012) 216–235.[33] H. Strasdat, A. J. Davison, J. M. Montiel, K. Konolige, Double window optimisation for constant time visual slam, in: 2011 internationalconference on computer vision, IEEE, 2011, pp. 2352–2359.[34] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, Keyframe-based visual–inertial odometry using nonlinear optimization, TheInternational Journal of Robotics Research 34 (3) (2015) 314–334.[35] T. Qin, P. Li, S. Shen, Vins-mono: A robust and versatile monocular visual-inertial state estimator, IEEE Transactions on Robotics 34 (4)(2018) 1004–1020.[36] S. Agarwal, Y. Furukawa, N. Snavely, I. Simon, B. Curless, S. M. Seitz, R. Szeliski, Building rome in a day, Communications of the ACM54 (10) (2011) 105–112.[37] B. Triggs, P. F. McLauchlan, R. I. Hartley, A. W. Fitzgibbon, Bundle adjustment—a modern synthesis, in: Vision algorithms: theory andpractice, Springer, 2000, pp. 298–372.[38] J.-L. Blanco, J.-L. Torres-Moreno, A. Gim´enez-Fern´andez, Multibody dynamic systems as bayesian networks: applications to robust stateestimation of mechanisms, Multibody System Dynamics 34 (2015) 103–128.