# A Factor-Graph Approach for Optimization Problems with Dynamics Constraints

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

## Key facts

- Cited by 5 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 11 Nov 2020.

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

- [Adversarial Motion Priors Make Good Substitutes for Complex Reward Functions](https://mlsanity.com/paper/2203.15103/) (2022)
- [VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots](https://mlsanity.com/paper/2107.07243/) (2021)
- [AstroSLAM: Autonomous Monocular Navigation in the Vicinity of a Celestial Small Body -- Theory and Experiments](https://mlsanity.com/paper/2212.00350/) (2022)
- [Safe Beyond the Horizon: Efficient Sampling-based MPC with Neural Control Barrier Functions](https://mlsanity.com/paper/2502.15006/) (2025)
- [LatentMimic: Terrain-Adaptive Locomotion via Latent Space Imitation](https://mlsanity.com/paper/2604.16440/) (2026)

## Abstract (arXiv)

In this paper, we introduce dynamics factor graphs as a graphical framework to solve dynamics problems and kinodynamic motion planning problems with full consideration of whole-body dynamics and contacts. A factor graph representation of dynamics problems provides an insightful visualization of their mathematical structure and can be used in conjunction with sparse nonlinear optimizers to solve challenging, high-dimensional optimization problems in robotics. We can easily formulate kinodynamic motion planning as a trajectory optimization problem with factor graphs. We demonstrate the flexibility and descriptive power of dynamics factor graphs by applying them to control various dynamical systems, ranging from a simple cart pole to a 12-DoF quadrupedal robot.