Patent ID: 8731880

Claim:
A method for calculating external forces 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 the plurality of rigid bodies including all joints connecting any two of the plurality of rigid bodies; (ii) specifying the trajectory for the plurality of rigid bodies over a series of points in time; (iii) at each point of said series of points in time, obtaining a current joint-space position, a velocity, and a next-step velocity for the plurality of rigid bodies from the specified trajectory; (iv) computing a joint-space inertia matrix for the plurality of rigid bodies; (v) computing a vector of non-contact forces that act on the plurality of rigid bodies and do not depend on the external forces; (vi) identifying all contact points among the plurality of rigid bodies; (vii) constructing a Jacobian that maps velocities from joint-space to contact-space; (viii) computing a contact-space inverse inertia matrix; (ix) using the computer system, computing a contact force at each of the contact points by minimizing a scalar objective function, wherein a minimizer of the objective function coincides with a solution to a corresponding forward dynamics problem for the plurality of rigid bodies; and (x) using the computer system, computing the external forces that, when applied to the plurality of rigid bodies, would produce the specified trajectory, as a function of the contact forces, the joint-space inertia matrix, the Jacobian, the velocity, the next-step velocity, and the vector of non-contact forces.