Fast simulation of Rigid Multibody Quasistatic Systems with Convex Optimization
Tao Pang and Russ Tedrake
Robot Locomotion Group
{pangtao, russt}@csail.mit.edu
Outline
- Simulation for robotic manipulation:
- Lots of contacts,
- Things move slowly.
- A fast time-stepping simulation scheme that combines
- Anitescu's Coulomb friction model,
- the simulation can be formulated as a convex quadratic program (QP).
- Simplifying assumptions for manipulation:
- The world is quasistatic,
- Robots are modeled as impedances.
- Anitescu's Coulomb friction model,
- Biggest advantage: use of larger (sometimes 100x) time steps than 2nd-order dynamics.
Simulating contacts
- Contact force/impulse and signed distance satisfy complementarity constraints:
- Optimization problems that have complementarity constraints are generally non-convex.
- In the frictionless case, complementarity constraints emerge as the KKT conditions of a convex quadratic program (QP).
Normal contact force/impulse.
Signed distance.
normal contact force
KKT conditions
spring force
complementary slackness
zero gradient (force balance)
primal and dual feasibility
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:
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335691/pasted-from-clipboard.png)
KKT condition of a convex QP.
- These are the standard LCP constraints for rigid mulitbody simulation, typically attributed to Stewart, Trinkle and Anitescu.
Anitescu, Mihai, and Florian A. Potra. "Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems." Nonlinear Dynamics 14.3 (1997): 231-247.
Stewart, David E. "Rigid-body dynamics with friction and impact." SIAM review 42.1 (2000): 3-39.
Modeling Coulomb friction as a convex program
- Anitescu proposed a relaxation of the standard complementarity constraints for frictional contacts.
- 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.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335809/pasted-from-clipboard.png)
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)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8337976/pasted-from-clipboard.png)
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 law 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.
Anitescu, Mihai. "Optimization-based simulation of nonsmooth rigid multibody dynamics." Mathematical Programming 105.1 (2006): 113-143.
Specializing to robotic manipulation
- Things move slowly (quasistatic)
- Robots can be modeled as impedances, which become springs under the quasistatic assumption.
- Treat robots (actuated) and objects (unactuated) 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.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8336338/pasted-from-clipboard.png)
Integral error vs. simulation time step
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8336350/pasted-from-clipboard.png)
Pose trajectory of the red box.
- Ground truth trajectory is generated by Drake's MultibodyPlant with h=5e-5.
Conclusion
- A fast convex time-stepping scheme for rigid multi-body systems with frictional contacts, which combines
- Anitescu's frictional constraints
- Simplifying quasstatic assumptions.
- Large increase in simulation time step, which not only makes simulation faster, but also increases the horizon of planning algorithms using the proposed dynamics model.
Backup slides
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:
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335636/pasted-from-clipboard.png)
- KKT conditions of the above QP include
- Contact forces (\(\lambda_1, \lambda_2, \lambda_3\)) emerge as Lagrange multipliers of the non-penetration constraints.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335641/pasted-from-clipboard.png)
Potential energy of the system.
Non-penetration constraints.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335649/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335650/pasted-from-clipboard.png)
Force balance of all boxes.
Unilateral contact constraints.
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.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8335727/pasted-from-clipboard.png)
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
Drawbacks of the quasistatic assumption.
- Cannot simulate dropping.
- Cannot simulate rolling a sphere on a flat surface.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8337021/pasted-from-clipboard.png)
Drawbacks of the quasistatic assumption.
- Solutions may not be unique.
- The quadratic form in the cost of the QP is positive semi-definite.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8151363/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8151364/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8336391/pasted-from-clipboard.png)
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.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/8338148/pasted-from-clipboard.png)
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_ICRA21
By Pang
quasistatic_sim_ICRA21
- 20