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

arXiv: https://arxiv.org/abs/2101.02874 · HTML: https://mlsanity.com/paper/2101.02874/

## Key facts

- Cited by 1 papers in the map (data through 14 Oct 2026).
- Builds on 5 of its 45 references that are in the map.
- First posted on arXiv 8 Jan 2021.

## Builds on (most cited first, 5 of 5)

- [VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator](https://mlsanity.com/paper/1708.03852/) (2017)
- [Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors](https://mlsanity.com/paper/1712.05873/) (2017)
- [On Computational Complexity Reduction Methods for Kalman Filter Extensions](https://mlsanity.com/paper/1512.03077/) (2015)
- [A Unified Method for Solving Inverse, Forward, and Hybrid Manipulator Dynamics using Factor Graphs](https://mlsanity.com/paper/1911.10065/) (2019)
- [Batch and Incremental Kinodynamic Motion Planning using Dynamic Factor Graphs](https://mlsanity.com/paper/2005.12514/) (2020)

## Built on by (most cited first, 1 of 1)

- [A Factor Graph-based approach to vehicle sideslip angle estimation](https://mlsanity.com/paper/2107.09815/) (2021)

## Abstract (arXiv)

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.