[Paper Review] A Unified Method for Solving Inverse, Forward, and Hybrid Manipulator Dynamics using Factor Graphs

Mandy Xie, Frank Dellaert|arXiv (Cornell University)|Nov 22, 2019

Robotic Mechanisms and Dynamics55 references4 citations

TL;DR

This paper presents a unified factor graph framework for solving inverse, forward, and hybrid manipulator dynamics problems in both open and closed kinematic chains. By modeling dynamics equations as factor graphs and leveraging variable elimination orderings, the method provides a visual, intuitive, and computationally flexible way to derive and implement classical and novel dynamics algorithms, with demonstrated efficiency through elimination ordering strategies like nested dissection.

ABSTRACT

This paper describes a unified method solving for inverse, forward, and hybrid dynamics problems for robotic manipulators with either open kinematic chains or closed kinematic loops based on factor graphs. Manipulator dynamics is considered to be a well studied problem, and various different algorithms have been developed to solve each type of dynamics problem. However, they are not easily explained in a unified and intuitive way. In this paper, we introduce factor graphs as a unifying graphical language in which not only to solve all types of dynamics problems, but also explain the classical dynamics algorithms in a unified framework.

Motivation & Objective

• To unify the solution of inverse, forward, and hybrid dynamics problems for robotic manipulators using a single graphical framework.
• To provide an intuitive, visual representation of complex dynamics equations through factor graphs, improving clarity over traditional algebraic formulations.
• To enable the derivation of new, efficient dynamics algorithms by exploring different variable elimination orderings in the factor graph.
• To extend the applicability of dynamics solvers to systems with kinematic loops, including overconstrained and redundantly actuated mechanisms.
• To lay the foundation for automatic code generation from topological elimination orderings in factor graphs.

Proposed method

• Model manipulator dynamics using factor graphs, where factors represent physical constraints (e.g., Newton-Euler equations) and variables represent state quantities (e.g., torques, accelerations, wrenches).
• Represent both open and closed kinematic chains by encoding joint dynamics, kinematic constraints, and loop closure conditions as factors in the graph.
• Use variable elimination (Gaussian elimination) on the factor graph to solve for unknowns, with elimination order determining algorithmic efficiency and structure.
• Apply graph partitioning techniques such as nested dissection to derive optimal or near-optimal elimination orderings that improve computational performance.
• Introduce specialized factors (e.g., planar factors for loop joints) to resolve underdetermined constraint forces in closed-loop systems and ensure unique solutions.
• Leverage the resulting directed acyclic graphs (DAGs) from elimination to generate efficient, topology-specific code for dynamics solvers.

Experimental results

Research questions

• RQ1 Can a single graphical framework unify the solution of inverse, forward, and hybrid dynamics problems across both open and closed kinematic chains?
• RQ2 How can factor graphs provide a more intuitive and visual representation of the underlying dynamics equations compared to traditional algebraic methods?
• RQ3 What is the impact of different variable elimination orderings on the efficiency and structure of derived dynamics algorithms?
• RQ4 Can factor graphs be used to systematically derive new, efficient dynamics algorithms for complex systems like parallel robots with kinematic loops?
• RQ5 To what extent can factor graph-based elimination lead to automatically generated, high-performance code for dynamics computation?

Key findings

• The factor graph framework successfully unifies the solution of inverse, forward, and hybrid dynamics problems across both open and closed kinematic chains in a single, coherent representation.
• Different elimination orderings in the factor graph correspond directly to known classical algorithms (e.g., RNEA, ABA, CRBA), revealing their underlying structure and relationship.
• Nested dissection-based elimination orderings significantly improve performance, with reported speedups over standard methods, though absolute timing is not the primary focus.
• The method enables the resolution of overconstrained and redundantly actuated systems by introducing additional factors (e.g., minimum torque, planar constraint factors) to ensure unique solutions.
• The resulting DAGs from elimination provide a natural basis for automatic code generation, potentially outperforming hand-optimized solvers when using non-intuitive but efficient orderings.
• The framework supports future extensions to kinodynamic motion planning and state estimation in dynamically balanced robots by integrating incremental inference.