Forward dynamics algorithms for multibody chains and contact

We describe a framework for derivation of several forward dynamics algorithms used in robotics. The framework is based on formulating an augmented system and performing block matrix elimination on this system. Several popular algorithms such as the O(N) articulated body method, and the composite rigid body method can be easily derived. We also derive an algorithm for simulation of contact between smooth bodies of arbitrary shape, in contact coordinates. Finally, we discuss some potential numerical difficulties that could arise and their solution