## Branch-Induced Sparsity

This project followed on from the condition-number project.  Having investigated the numerical properties of the joint-space inertia matrix (JSIM), I decided to take a look at the sparsity patterns (patterns of permanently zero-valued elements) in this matrix.

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.

## Divide-and-Conquer Articulated-Body Dynamics

The objective was to create an algorithm that would calculate the forward dynamics of a large system of rigid bodies very quickly on a parallel computer.  I developed an algorithm that solves this problem by a recursive divide-and-conquer technique: the given rigid-body system is cut into two subsystems, the dynamics of each subsystem is worked out in parallel (by recursive application of the divide-and-conquer algorithm), and the motion of the original system is worked out from the dynamics of the two subsystems.

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.

## Improved Constraint-Force Algorithm

(A collaboration with Dr. Amir Fijany, who was at NASA JPL at the time)

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.

## Hybrid Motion/Force Control

(A collaboration with Prof. Oussama Khatib of Stanford University)

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.

## Condition Number of the Joint-Space Inertia Matrix

It is well known that the joint-space inertia matrix (JSIM) of a robot mechanism is a symmetric, positive-definite matrix, and that such matrices are guaranteed to be nonsingular.  However, it turns out that the condition number of the JSIM can be extremely large.  For example, given a robot mechanism consisting of an unbranched chain of identical links connected by revolute joints, the JSIM for this mechanism can have a condition number as high as 4N4, where N is the number of bodies in the chain.  Thus, even a chain with only 10 bodies can exhibit a condition number as high as 40,000, and the problem quickly gets worse as N increases.

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.