Efficient Factorization of the Joint-Space Inertia Matrix for Branched Kinematic Trees

Abstract

This paper describes new factorization algorithms that exploit branch-induced sparsity in the joint-space inertia matrix (JSIM) of a kinematic tree. It also presents new formulae that show how the cost of calculating and factorizing the JSIM vary with the topology of the tree. These formulae show that the cost of calculating forward dynamics for a branched tree can be considerably less than the cost for an unbranched tree of the same size. Branches can also reduce complexity; some examples are presented of kinematic trees for which the complexity of calculating and factorizing the JSIM are less than O(n2) and O(n3) , respectively. Finally, a cost comparison is made between an O(n) algorithm and an O(n3) algorithm, the latter incorporating one of the new factorization algorithms. It is shown that the O(n3) algorithm is only 15% slower than the O(n) algorithm when applied to a 30-degrees-of-freedom humanoid, but is 2.6 times slower when applied to an equivalent unbranched chain. This is due mainly to the O(n3) algorithm running about 2.2 times faster on the humanoid than on the chain.