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

- Anitescu's Coulomb friction model,
- 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:

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

contact force

Forces along extreme rays of the friction cone.

Signed distance at the current time step.

Translation along the extreme rays.

Time step.

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

A friction cone with 2 extreme rays:

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

Jacobians along the contact normal.

Jacobian along the contact tangents.

Generalized velocity of the system.

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

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 (
**a**ctuated) and objects (**u**nactuated) differently:

Coriolis

gravity

contact

mass

actuation

Objects dynamics:

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

Robot dynamics:

Commanded joint angles at the next time step.

Gravity compensation

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

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

Coriolis

gravity

contact

mass

actuation

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

Robot dynamics:

Objects dynamics:

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

#### quasistatic_sim_march_2021

By Pang

# quasistatic_sim_march_2021

- 346