Invertible contact model
View Patent ↗Methods for computing the inverse dynamics of multibody systems with contacts are disclosed. Inverse dynamics means computing external forces that cause a system to move along a given trajectory. Such computations have been used routinely for data analysis and control synthesis in the absence of contacts between rigid bodies. The disclosed inverse dynamics methods include the ability to handle contacts. The disclosed methods include the following steps: projecting the discrete-time equations of motion from joint space to contact space; defining the forward dynamics in contact space as the solution to an optimization problem; using the features of this optimization problem to obtain a unique inverse—which turns out to correspond to the solution to a dual optimization problem; solving the latter using standard methods for numerical optimization; projecting the solution from contact space back to joint space and finding the external forces. The corresponding methods for computing forward dynamics are also disclosed.
1. 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.
2. The method of claim 1 , wherein the external forces are calculated as
u=M λ h −1 ( w′−w )− M λ M −1 ( n+h −1 J T f )
wherein:
u represents the external forces,
h is a discretization time step,
w represents the velocity,
w′ represents the next-step velocity,
M represents the joint-space inertia matrix,
n represents the vector of non-contact forces,
J represents the Jacobian,
f represents the contact forces, and
M λ =M+λJ n T J n ,
wherein J n is a sub-Jacobian matrix corresponding to normal directions of the contact points, and λ is a non-negative user-specified parameter,
and further wherein the superscript T indicates the transpose operation and the superscript −1 indicates the inverse operation.
3. The method of claim 1 , wherein the non-contact forces comprise at least one of a Coriolis force, a centrifugal force, a gravitational force, a drag force, or a spring-damper force.
4. The method of claim 1 , wherein the step of minimizing the scalar objective function is performed using a convex contact solver.
5. The method of claim 1 , wherein the step of minimizing the scalar objective function is performed using an implicit contact solver.
6. The method of claim 1 , wherein the plurality of rigid bodies comprise a robotic system, and further wherein the external forces are applied to the rigid bodies with controllable actuators.
7. A method for calculating a trajectory of a plurality of rigid bodies that would be produced by a time-series of external forces, 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 time-series of external forces to be applied to 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 and a current joint-space velocity from one of an initial condition and results of a previous time step, and the external forces of the time-series of external forces acting during a current time step;
(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, using one of
(A) a convex contact solver that minimizes a kinetic energy in said contact-space; and
(B) an implicit contact solver that represents both the contact force and a next-step velocity calculated for the next time step at said current time step in said contact-space parametrically as functions of one independent variable;
(x) using the computer system, computing said next-step velocity as a function of the contact forces, the joint-space inertia matrix, the Jacobian, the current joint-space velocity, and the vector of non-contact forces, and integrating said next-step velocity to obtain a next-state position.
8. The method of claim 7 , wherein said next-step velocity is computed as
w′=w+M −1 ( hn+J T f )+ M λ −1 hu
wherein w′ represents the next-step velocity,
w represents the current joint-space velocity,
M represents the joint-space inertia matrix,
h is a discretization time step,
n represents the vector of non-contact forces,
J represents the Jacobian,
u represents the time-series of external forces,
f represents the contact forces, and
M λ =M+λJ n T J n ,
wherein is a sub-Jacobian matrix corresponding to normal directions of the contact points, and λ is a non-negative user-specified parameter,
and further wherein the superscript T indicates the transpose operation and the superscript −1 indicates the inverse operation.
9. The method of claim 7 , wherein the non-contact forces comprise at least one of a Coriolis force, a centrifugal force, a gravitational force, a drag force, or a spring-damper force.
10. The method of claim 7 , wherein the plurality of rigid bodies comprise a robotic system, and further wherein the external forces are applied to the rigid bodies with controllable actuators.
11. The method of claim 7 , further comprising generating an animated sequence showing the calculated trajectory of the plurality of rigid bodies.
12. A method for controlling a robotic system comprising a plurality of rigid members along a prescribed trajectory comprising:
(i) on a computer system, defining a model of the robotic system comprising the plurality of rigid members including all joints connecting any two of the plurality of rigid members;
(ii) specifying the trajectory for the plurality of rigid members 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 members from the specified trajectory;
(iv) computing a joint-space inertia matrix for the plurality of rigid members;
(v) computing a vector of non-contact forces that act on the plurality of rigid members;
(vi) identifying all contact points among the plurality of rigid members;
(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 members; and
(x) using the computer system, computing external forces that, when applied to the plurality of rigid members, 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.
13. The method of claim 12 , wherein the external forces are calculated as
u=M λ h −1 ( w′−w )− M λ M −1 ( n+h −1 J T f )
wherein M λ =M+λJ n T J n and J n is a sub-Jacobian matrix corresponding to normal directions of the contact points, and λ is a non-negative user-specified parameter;
and further wherein:
u represents the external forces,
h is a discretization time step,
w represents the velocity,
w′ represents the next-step velocity,
M represents the joint-space inertia matrix,
n represents the vector of non-contact forces,
J represents the Jacobian, and
f represents the contact forces,
and further wherein the superscript T indicates the transpose operation and the superscript −1 indicates the inverse operation.
14. The method of claim 12 , wherein the non-contact forces comprise at least one of a Coriolis force, a centrifugal force, a gravitational force, a drag force, or a spring-damper force.
15. The method of claim 12 , wherein the step of minimizing the scalar objective function is performed using a convex contact solver.
16. The method of claim 12 , wherein the step of minimizing the scalar objective function is performed using an implicit contact solver.
17. The method of claim 12 , wherein the plurality of rigid members comprise a robotic system, and further wherein the external forces are applied to the rigid members with controllable actuators.