A general framework for modeling and dynamic simulation of multibody
  systems using factor graphs
  In this paper, we present a novel general framework grounded in the factor
graph theory to solve kinematic and dynamic 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 intuitive interpretation is still pursued. We describe
how to build factor graphs to model and simulate multibody systems using both,
independent and dependent coordinates. Then, batch optimization or a
fixed-lag-smoother can be applied to solve the underlying optimization problem
that results in a highly-sparse nonlinear minimization problem. The proposed
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 accuracy with classical approaches. The proposed factor graph-based
framework has the potential to be integrated into applications related with
motion estimation and parameter identification of complex mechanical systems,
ranging from mechanisms to vehicles, or robot manipulators.
