# 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.

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)!

By Pang

• 166