The term kinematic tree describes a rigid-body system in which the bodies are connected to form a topological tree (i.e., there are no kinematic loops). Branches in the tree automatically give rise to a sparsity pattern in the JSIM which I decided to call branch-induced sparsity. It turns out that we can exploit this sparsity, using very simple algorithms, to reduce both the cost of factorizing the JSIM, and the cost of back-substitution. Furthermore, even the complexity of factorization is reduced from O(n3) to O(nd2), where n is the degree of freedom (DoF) of the system and d is the depth of the tree. (The JSIM is an n×n matrix.) The reduction in computational cost can be dramatic: roughly a factor of 4 for a 30-DoF quadruped, which is enough to bring JSIM-based algorithms back into contention with O(n) algorithms like the articulated-body algorithm. This work was published here, and it also appears in my new book. Implementations of the factorization and back-substitution algorithms are included in the software package Spatial.
After a lapse of a few years, I returned to this project to investigate whether branch-induced sparsity could reduce the cost of calculating the operational-space inertia matrix (OSIM). This work turned out to be even more successful, yielding a factor of 6 reduction in the cost of calculating the OSIM for an Asimo next-generation robot (a typical high-end humanoid). Furthermore, it beat the best previously-published algorithm for this computation. This work was published here.
All that now remains to be done is to investigate the exploitation of branch-induced sparsity in systems containing kinematic loops.
The result is an algorithm that takes O(log(N)) elapsed time to calculate the dynamics of a system containing N rigid bodies on a parallel computer with O(N) processors. The new algorithm is faster than other published parallel algorithms, but its numerical accuracy is not as good as the best serial algorithms. (To be fair, other parallel algorithms also suffer from poor numerical accuracy.)
This work was published in a two-part paper that has now been quite heavily cited: the divide-and-conquer algorithm was the first of a new class of dynamics algorithm, and it opened the door to a variety of opportunities to adapt and extend the basic idea in new directions.
The Constraint Force algorithm (CFA) developed by Dr. Fijany et al. was the first published dynamics algorithm to achieve O(log(N)) time complexity for a chain of N rigid bodies on a parallel computer with O(N) processors. The original version was applicable only to systems in which the bodies were connected together in an unbranched chain, using a restricted set of joint types. In this project, we developed a new version that overcame these restrictions; and the work was published here.
Perhaps more important than the algorithm itself, is the technique used to derive it. I called it the change-of-basis technique, but it is really a general method for the mathematical analysis of motion and force constraints in a vector space and its dual. I used it subsequently in the hybrid motion/force control project.
The purpose of this project was to replace the Raibert/Craig contact model used in hybrid motion/force control with a more general contact model that could model kinematic constraints within the robot mechanism (i.e., an end-effector with fewer than 6 degrees of freedom), general kinematic constraints between the robot and its environmemt, as well as general instantaneous dynamics of the environment (including unknown instantaneous dynamics). One particularly noteworthy property of this model is that it dynamically decouples the force and motion control channels even in the face of unknown environment dynamics. The new model we developed is theoretically sound, and those parts of it that have been experimentally verified indicate that the model also works in practice. This project yielded two conference papers (#1, #2) and one journal article.
This is bad news for anyone wishing to simulate or control a rigid-body system with a large number of bodies. Simulators suffer a loss of efficiency, accuracy, or both, depending on integration step size, while control systems must cope with a highly sensitive plant in which tiny errors in applied forces can produce large acceleration errors. Fortunately, the picture is not this bad for every mechanism. Unbranched chains appear to be the worst case. Furthermore, the usual definition of matrix condition number (ratio of largest and smallest singular values) can overestimate the true scale of the problem in some cases. A more accurate measure is the condition number of a scaled version of the JSIM in which the diagonal elements have all been brought to the same value (e.g. 1). This work was published here.