Patent Document ID: 20120150499
Application ID: 13232954
Patent Flag: 0

Claim One:
1. A method for calculating external forces (u) that when applied to a plurality of rigid bodies would produce a specified trajectory of the rigid bodies, the method comprising: (i) on a computer system, defining models of a plurality of rigid bodies including all joints connecting any two of the plurality of rigid bodies; (ii) specifying a trajectory for the plurality of rigid bodies over a series of points in time; (iii) at each point in time, obtaining a current joint-space position (q), a velocity (w), and a next-step velocity (w′) for the plurality of rigid bodies from the specified trajectory; (iv) computing a joint-space inertia matrix (III) for the plurality of rigid bodies; (v) computing a vector of non-contact forces (n) that act on the plurality of rigid bodies and do not depend on the external force; (vi) identifying all contact points among the plurality of rigid bodies; (vii) constructing a Jacobian (J) that maps velocities from joint-space to contact-space; (viii) computing a contact-space inverse inertia matrix (A); (ix) computing a contact force (f) at each of the contact points by minimizing a scalar objective function (L inv ), wherein the minimizer of the objective function coincides with the solution to a corresponding forward dynamics problem for the plurality of rigid bodies; and (x) computing the external forces (u) that when applied to the plurality of rigid bodies would produce the specified trajectory, as a function of f, M, J, w, w′, and n.