Computation reuse for rigid-body dynamics

The accelerations of and forces among contacting rigid bodies may be computed by formulating the dynamics equations and contact constraints as a complementarity problem (P. Lotstedt, 1981). Dantzig's algorithm, when applicable, will find a solution to the linear complementarity problem corresponding to an assembly with n contacts in O(n) major cycles. Can the dynamics of an assembly be computed more quickly if the dynamics of a subassembly are already known? This paper shows that Dantzig's algorithm will find a solution in O(n - k) major cycles if the algorithm is initialized with a solution to the dynamics problem for a subassembly with k internal contacts. We apply this observation to two robotics problems: dynamic simulation and assembly sequence planning. In dynamic simulation, the positions of several bodies might remain fixed during a sequence of frames. We compute the dynamics of this motionless subset (which might not be motionless when considered in isolation), and use the result to initialize the computation for the entire assembly. In assembly planning, non-disjoint sets of objects are typically considered sequentially by the planner. If the configuration of only one body is varied, the dynamics of successive assemblies can be computed in a constant number of major cycles