Coordinate-invariant algorithms for robot dynamics

We present, using methods from the theory of Lie groups and Lie algebras, a coordinate-invariant formulation of the dynamics of open kinematic chains. We first re-formulate the recursive dynamics algorithm originally given by Park et al. (1995) for open chains in terms of standard linear operators on the Lie algebra of the special Euclidean group. Using straight forward algebraic manipulations, we then recast the resulting algorithm into a set of closed-form dynamic equations. We then reformulate Featherstone's (1987) articulated body inertia algorithm using this same geometric framework, and re-derive Rodriguez et al.'s (1991, 1992) square factorization of the mass matrix and its inverse. An efficient O(n) recursive algorithm for forward dynamics is also extracted from the inverse factorization. The resulting equations lead to a succinct high-level description of robot dynamics in both joint and operational space coordinates that minimizes symbolic complexity without sacrificing computational efficiency, and provides the basis for a dynamics formulation that does not require link reference frames in the description of the forward kinematics.