Simulating Rigid Multibody Dynamics with Convex Optimization

Pang

Outline

A time-stepping simulation scheme that combines
- Anitescu's Coulomb friction model,
  - the simulation can be solved as a convex QP.
- Simplifying assumptions:
  - Quasistatic,
  - Robot is impedance-controlled.
Biggest advantage: the use of larger time steps than 2nd-order dynamics.
Drawbacks of the quasistatic assumption and improvements.

Simulating contacts

Contact force/impulse and signed distance satisfy complementarity constraints:

0 \leq \lambda_{n_i} \perp \phi_i \geq 0

Optimization problems that have complementarity constraints are generally non-convex.

For contact \(i\), the unilateral contact constraint is

Contact force/impulse.

Signed distance.

Simulating contacts

In the frictionless case, we do not need to write down complementarity constraints explicitly.
Example from Boyd's convex optimization textbook: find the stable configuration of the following system:

KKT conditions of the above QP include

Contact forces (\(\lambda_1, \lambda_2, \lambda_3\)) emerge as Lagrange multipliers of the non-penetration constraints.

Potential energy of the system.

Non-penetration constraints.

Force balance of all boxes.

Unilateral contact constraints.

Simulating contacts

But for frictional contacts, contact forces need to satisfy additional constraints:
- Friction cone,
- maximum dissipation principle.
These additional constraints mean that contact forces need to be explicitly included in the optimization as decision variables, making the problem non-convex:

KKT condition of a convex QP.

These are the standard LCP constraints for rigid mulitbody simulation, typically attributed to Stewart, Trinkle and Anitescu.

Modeling Coulomb friction as a convex program

Anitescu proposed a tighter relaxation of the standard complementarity constraints for frictional contact.
Example: frictional contact \(i\) in 2D with friction coefficient \(\mu_i\):

Anitescu, Mihai. "Optimization-based simulation of nonsmooth rigid multibody dynamics." Mathematical Programming 105.1 (2006): 113-143.

\beta_{i1}

\beta_{i2}

\mu \beta_{i1}

\mu \beta_{i2}

contact force

h(v_{n_i} - \mu_i v_{d_i}) + \phi_i \geq 0

h(v_{n_i} + \mu_i v_{d_i}) + \phi_i\geq 0

\text{force} \propto \beta_{i2}

\text{force} \propto \beta_{i1}

\bm{\mathsf{n}}_i

\bm{\mathsf{d}}_{i2}

\bm{\mathsf{d}}_{i1}

0 \leq \beta_{i1} \perp h(v_{n_i} + \mu_i v_{d_i}) + \phi_i \geq 0,\\ 0 \leq \beta_{i2} \perp h(v_{n_i} - \mu_i v_{d_i}) + \phi_i \geq 0.

Forces along extreme rays of the friction cone.

Signed distance at the current time step.

Translation along the extreme rays.

Time step.

\phi_i = 0

Anitescu's constraints

are the same as the standard complementarity constraints when the contact is not sliding.
creates a gap between two objects when they are sliding relative to each other.

Modeling Coulomb friction as a convex program

Standard LCP (a)

Anitescu (b)

Modeling Coulomb friction as a convex program

MuJoCo also solves multi-body contact forward simulation as a convex QP, but doesn't accurately model contact forces.

Kolbert, Roman, Nikhil Chavan-Dafle, and Alberto Rodriguez. "Experimental validation of contact dynamics for in-hand manipulation." International Symposium on Experimental Robotics. Springer, Cham, 2016.

standard LCP contact model

Modeling Coulomb friction as a convex program

0 \leq \beta_{i1} \perp h(v_{n_i} + \mu_i v_{d_i}) + \phi_i \geq 0,\\ 0 \leq \beta_{i2} \perp h(v_{n_i} - \mu_i v_{d_i}) + \phi_i \geq 0.

A friction cone with 2 extreme rays:

A friction cone with \(n_d\) extreme rays:

0 \leq \beta_{ij} \perp \left(J_{n_i} + \mu_i J_{d_{ij}}\right)v + \phi_i / h \geq 0, \forall j = 1 \dots n_d.

Jacobians along the contact normal.

Jacobian along the contact tangents.

Generalized velocity of the system.

M(q)(v' - v) = -hC(q, v) + h\tau_g + h\tau_a + \sum_{ij} \left(J_{n_i} + \mu_i J_{d_{ij}}\right)^\intercal \beta_{ij} \\ \text{subject to} \\ 0 \leq \beta_{ij} \perp \left(J_{n_i} + \mu_i J_{d_{ij}}\right)v + \phi_i / h \geq 0, \forall j = 1 \dots n_d.

Newton's second with contact becomes:

Coriolis

gravity

contact

mass

velocity, next step

velocity, current

configuration, current

which are the KKT conditions of the following convex QP:

actuation

J_{ij}

\underset{v'}{\min} \; \frac{1}{2} v'^\intercal M v' + \left(-h\tau_g - h\tau_a - Mv + hC \right)^\intercal v'\\ \text{subject to} \\ \frac{\phi_i}{h} + J_{ij} v' \geq 0, \forall i, j.

contact index.

Extreme ray index.

Specializing to robotic manipulation

Things move slowly (quasistatic)
Robot is impedance controlled, which becomes a spring under the quasistatic assumption.
Treat robots (actuated) and objects (unactuated) differently:

M(q)(v' - v) = -hC(q, v) + h\tau_g + h\tau_a + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Coriolis

gravity

contact

mass

actuation

q = \begin{pmatrix} q_u \\ q_a \end{pmatrix}, v = \begin{pmatrix} v_u \\ v_a \end{pmatrix}

\underset{v'}{\min} \; \frac{h^2}{2} v_a'^\intercal K v_a' - h \begin{bmatrix} \tau_{g_u} \\ K_a (\bar{q}_a' - q_a) \end{bmatrix}^\intercal \begin{bmatrix} v_u'\\ v_a' \end{bmatrix}\\ \text{subject to} \\ \frac{\phi_i}{h} + J_{ij} v' \geq 0, \forall i, j.

\implies 0 = \tau_{g_u} + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Objects dynamics:

\tau_a = 0

Dynamics of the entire system is given by the KKT conditions of

\tau_a = K_a \left(\bar{q}_a' - (q_a + h v_a') \right) - \tau_g

Robot dynamics:

Commanded joint angles at the next time step.

Gravity compensation

\implies 0 = hK_a \left(\bar{q}_a' - (q_a + h v_a') \right) + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Simulation results

The Anitescu contact model and the quasistatic simplification allows much larger simulation time steps.

Quasistatic, h = 0.1s.

MBP, h = 1e-3s.

Integral error vs. simulation time step

Pose trajectory of the red box.

Ground truth trajectory is generated by MBP with h=5e-5.

Drawbacks of the quasistatic assumption.

Cannot simulate dropping.
Cannot simulate rolling a sphere on a flat surface.

Drawbacks of the quasistatic assumption.

Solutions may not be unique.
- The quadratic form in the cost of the QP is positive semi-definite.

\underset{v'}{\min} \; \frac{h^2}{2} v_a'^\intercal K v_a' + h \begin{bmatrix} \tau_{g_u} \\ K_a (\bar{q}_a' - q_a) \end{bmatrix}^\intercal \begin{bmatrix} v_u'\\ v_a' \end{bmatrix}\\ \text{subject to} \\ \frac{\phi_i}{h} + J_{ij} v' \geq 0, \forall i, j.

Specializing to robotic manipulation, take 2.

Robots are still impedance-controlled and modeled as springs.
Objects are quasi-dynamic.
- First proposed by Matt Mason: objects move slowly and "in the direction of acceleration".
- An impulse-momentum interpretation better illustrates why it's a good idea.

M(q)(v' - v) = -hC(q, v) + h\tau_g + h\tau_a + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Coriolis

gravity

contact

mass

actuation

\underset{v'}{\min} \; \frac{1}{2} \begin{bmatrix} v_u'\\ v_a' \end{bmatrix}^\intercal \begin{bmatrix} M_u & 0 \\ 0 & h^2K_a \end{bmatrix} \begin{bmatrix} v_u'\\ v_a' \end{bmatrix} - h \begin{bmatrix} \tau_{g_u} \\ K_a (\bar{q}_a' - q_a) \end{bmatrix}^\intercal \begin{bmatrix} v_u'\\ v_a' \end{bmatrix}\\ \text{subject to} \\ \frac{\phi_i}{h} + J_{ij} v' \geq 0, \forall i, j.

Dynamics of the entire system is given by the KKT conditions of

Robot dynamics:

\tau_a = K_a \left(\bar{q}_a' - (q_a + h v_a') \right) - \tau_g\\ M_a(v_a' - v_a) = 0

\implies 0 = hK_a \left(\bar{q}_a' - (q_a + h v_a') \right) + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Objects dynamics:

\tau_a = 0 \\ v_u = 0

\implies M_u v_u' = \tau_{g_u} + \sum_{ij} J_{ij}^\intercal \beta_{ij} \\

Impulses that cannot be balanced are converted into momentum.
The momentum is thrown away at the next time step.
Introduces a lot of damping, which seems reasonable for manipulation.

Simulation results

Quasistatic sphere pushing

Quasi-dynamic sphere pushing

Quasi-dynamic dropping

Also works for more complex systems (e.g. the example at the beginning)!