Planning, Control and Sensing for Contact-rich Robotic Manipulation

Tao Pang

Oct 2022

Why contact-rich?

Collision-free Motion Planning

Interact with the world

Smaller

What about RL (reinforcement learning)?

  • RL generates robust policies for specific tasks, but...
    • RL requires heavy offline computation,
    • the learned policy does not generalize (e.g. to opening a door).

 

We believe that understanding in model-based methods can lead to insights in RL, and vice versa. 

  1. (OpenAI) Andrychowicz et al. "Learning dexterous in-hand manipulation." The International Journal of Robotics Research, 2020.
  2. (Nvidia) Handa et al. "DeXtreme: Transfer of Agile In-hand Manipulation from Simulation to Reality", arXiv preprint, 2022.

[1] OpenAI.

[2] Nvidia.

Why is contact-rich planning hard?

Contact dynamics is non-smooth!

Two solutions

Descend with gradients of smoothed dynamics

Reason explicitly about contact mode transitions

q^\mathrm{a}_+
u

Contact

No Contact

  • But descent methods get stuck in local minima.
  • Therefore they need non-trivial initialization.
  • Good at escaping local minima,
  • But mode enumeration scales poorly (exponentially) with the number of contacts.
x_+ = u
x_+ = 0
u > 0
u \leq 0
q^\mathrm{a}_+
u

smoothed dynamics

original dynamics

\(u = 0\)

Model-based Planning through Contact: a Quick Review

  • Trajectory Optimization through Contact
  1. Mordatch, Igor, Emanuel Todorov, and Zoran Popović. "Discovery of complex behaviors through contact-invariant optimization." ACM Transactions on Graphics (TOG), 2012.
  2. Hogan, François Robert, and Alberto Rodriguez. "Feedback control of the pusher-slider system: A story of hybrid and underactuated contact dynamics." arXiv preprint arXiv:1611.08268, 2016.
  3. Posa, Michael, Cecilia Cantu, and Russ Tedrake. "A direct method for trajectory optimization of rigid bodies through contact." The International Journal of Robotics Research, 2014.
  4. Aydinoglu, Alp, and Michael Posa. "Real-time multi-contact model predictive control via admm." 2022 International Conference on Robotics and Automation (ICRA), 2022.
  5. Cheng, Xianyi, et al. "Contact mode guided motion planning for quasidynamic dexterous manipulation in 3d." 2022 International Conference on Robotics and Automation (ICRA), 2022.
  6. Marcucci, Tobia, et al. "Approximate hybrid model predictive control for multi-contact push recovery in complex environments." 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), 2017.
  • Enumerate mode switches locally
  • Enumerate Mode Switches
  • Sampling mode switches

Good 

Scalability

Bad Scalability

Local Planning

Global Planning

Global planning + Smoothed dynamics?

[1] Mordatch et al.

[3] Posa et al.

[5] Cheng et al.

[6] Marcucci et al.

[4] Aydinoglu and Posa.

[2] Hogan and Rogriguez.

Our Method: Sim and Hardware Demos.

  • Our planner combines sampling-based global search and smoothed contact dynamics.
  • Our planner takes less than a minute to find most plans shown above.

Open-loop execution of contact-rich plans on hardware:

Contact-rich manipulation also needs control and sensing

A QP controller that bounds contact forces while tracking a trajectory. [1]

Detecting contact forces from joint torques is tricky and unreliable.[2]

  1. Tao Pang, Russ Tedrake, "Easing Reliance on Collision-free Planning with Contact-aware Control", ICRA, 2022.

  2. Tao Pang, Jack Umenberger, Russ Tedrake, "Identifying external contacts from joint torque measurements on serial robotic arms and its limitations", ICRA,2021.

Presented Works

Collaborators

  • Tao Pang*, H.J. Terry Suh*, Lujie Yang, Russ Tedrake, "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models", under review.

  • H.J. Terry Suh*, Tao Pang*, Russ Tedrake, "Bundled Gradients through Contact via Randomized Smoothing", RA-L, 2022.

  • Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

  • Tao Pang, Russ Tedrake, "Easing Reliance on Collision-free Planning with Contact-aware Control", ICRA, 2022.

  • Tao Pang, Jack Umenberger, Russ Tedrake, "Identifying external contacts from joint torque measurements on serial robotic arms and its limitations", ICRA,2021.

Terry Suh

Lujie Yang

Russ Tedrake

Jack Umenberger

Opinion: we need robots designed for contact-rich interactions

  1. Hogan, Francois R., et al. "Tactile dexterity: Manipulation primitives with tactile feedback." 2020 IEEE international conference on robotics and automation (ICRA). IEEE, 2020.
  2. Suh, H. J., et al. "SEED: Series Elastic End Effectors in 6D for Visuotactile Tool Use." arXiv preprint arXiv:2111.01376, 2021.
  3. Luo, Yiyue, et al. "Learning human–environment interactions using conformal tactile textiles." Nature Electronics, 2021.

Human 

OpenAI

Nvidia

This robot needs full-body tactile sensing, ...

Joint torque

Hogan et al.

Hogan et al.[1]

Suh et al.[2]

"Camera under the fingertip" 

Luo et al.[3]

Tactile skin

Opinion: we need robots designed for contact-rich interactions

  1. Hwangbo, Jemin, et al. "Learning agile and dynamic motor skills for legged robots." Science Robotics 4.26 (2019): eaau5872.
  2. Salisbury, Kenneth, et al. "Preliminary design of a whole-arm manipulation system (WAMS)." Proceedings. 1988 IEEE International Conference on Robotics and Automation. IEEE, 1988.

"Probably of highest importance has been a focus on force control. Since the system is intended to contact objects, sometimes unpredictably, with any part of its mechanism, it is important that forces anywhere along the links be controllable." -- Ken Salisbury on whole-arm manipulation, 1988 [2].

 This robot also needs precisely-modeled compliance.

Other interesting things I did

  • Take-off weight: 10kg.
  • Maximum endurance: 3 hours.

Gasoline-engine-powered Quadcopter

3D Geometry from NeRF

I wrote the code that rendered this image.

Active SLAM of a planar robot

Why the AI Institute? 

My goals

the AI Institute's goals

Goal: a robot that can interact with anything using any part of its body.

  • Robust planning and control for contact-rich manipulation.
  • Human-level dexterity? Seems very natural after human-level locomotion!
  • Working with hardware team to design next-gen robots for contact-rich interactions.
  • Athletic intelligence + organic hardware design.
  • Components of the contact-rich planning pipeline can benefit from learning.
  • Improving model-based methods by learning and vice versa.
  • Athletic intelligence + Cognitive intelligence.

What is quasi-static dynamics?

q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
  • Quasi-static: velocity is small so that inertial and Coriolis forces are negligible.
  • \(x \coloneqq q \coloneqq  [q^\mathrm{u}, q^\mathrm{a}]\): system configuration.
    • \(q^\mathrm{a}\): actuated, imepdance-controlled robots
    • \(q^\mathrm{u}\): un-actuated objects.
  • \( u \): robot velocities position command to the stiffness-controlled robots [1].
  • \(\delta q \coloneqq q_+ - q\) 
  • \(h\): step size in seconds.

\( x_{+} = f(x, u)\)

\lambda_1
\lambda_3
\lambda_1
\lambda_2
  • \(\lambda_i\): contact forces/impulses
  • \(\phi_i \): signed distances
\phi_2({q})
\phi_1({q})
\phi_3({q})
\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
\Leftrightarrow

(KKT)

  • A convex quadratic program (QP), after linearizing \(\phi_i\).
  • "Minimizing potential energy, subject to non-penetration constraints."
\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ 0 = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0 \; \forall i, \\ \phi (q + \delta q) \lambda = 0, \forall i. \end{cases}

Force balance of the robot.

Force balance of the object.

Non-penetration.

"Contact forces cannot pull."

"Contact force needs contact."

  1. Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

Quasi-static dynamics is good for manipulation planning!

  • Many manipulation tasks are quasi-static.
  • Spatial benefit: Smaller state space (\([q, v]\) vs. only \(q\)).

  • Temporal benefit: Ignoring transients => looking into the future with fewer steps.

  1. Tao Pang, Russ Tedrake, "A Convex Quasistatic Time-stepping Scheme for Rigid Multibody Systems with Contact and Friction", ICRA, 2021.

q^\mathrm{a}
m^\mathrm{a}
u
K
D

Second-order Dynamics

Quasi-static Dynamics

\begin{aligned} m^\mathrm{a} &= 1.0 \\ D &= 20.0\\ K &= 100.0 \end{aligned}

Quasi-static Dynamics Needs Regularization

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}

Quasi-static Dynamics

  • The quadratic cost is positive semi-definite.
  • The object can be anywhere between the robot and the wall, similar to having a null space.
\underset{x}{\min} \|Ax - b\|^2
\underset{x}{\min} \|Ax - b\|^2 + \epsilon \|x\|^2

Regularized least squares

Regularized Quasi-static Dynamics

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \epsilon (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
  • The quadratic cost is now positive definite.
  • "Among all possible object motions, give me the one that moves the least."
  • Picking \(\epsilon = m^\mathrm{u} / h\) gives Mason's definition of quasi-dynamic dynamics.
q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
\phi_2({q})
\phi_1({q})
\phi_3({q})

What about friction?

\begin{aligned} \underset{\delta q}{\min} \; &\frac{1}{2} {\underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q}}^\intercal \underbrace{ \begin{bmatrix} \mathbf{M}_\mathrm{u} / h & 0 \\ 0 & h \mathbf{K}_\mathrm{a} \end{bmatrix}}_{\mathbf{Q}} \underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q} + {\underbrace{ h \begin{bmatrix} - \tau^\mathrm{u} \\ - \mathbf{K}_\mathrm{a} (u - q^\mathrm{a}) - \tau^\mathrm{a} \end{bmatrix}}_{b}}^\intercal \underbrace{ \begin{bmatrix} \delta q^\mathrm{u} \\ \delta q^\mathrm{a} \end{bmatrix}}_{\delta q} \;\text{s.t.} \\ &\underbrace{\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q}_{\text{Linearized} \; \phi_i(q + \delta q)} \geq 0, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
  • For a frictionless multi-body system with \(n_\mathrm{c}\) contacts, the dynamics can be solved as a QP:
  • \(\mathcal{K}^\star_i\) is the dual of the \(i\)-th friction cone.
  • Still convex! Convexity is achieved at the expense of "hydroplaning" between relatively-sliding objects.
  • But hydroplaning disappears as \(h \rightarrow 0\).
\begin{aligned} \underset{\delta q}{\min} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
  • Anitescu [1] has a nice convex approximation of Coulomb friction constraints, allowing us to write down the dynamics as a Second-Order Cone Program (SOCP):

Tangential displacements

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \; \forall i. \end{aligned}

Dynamics of the Toy Problem

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

Differentiability

\begin{aligned} \delta q^\star \coloneqq \underset{\delta q}{\text{argmin}.} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}

Convex, Quasi-Static, Differentiable Dynamics (an SOCP)

q_+ = f(q, u) = q + \delta q^\star(q, u)
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} \mathbf{A} \coloneqq \DfDx{f}{q} = \mathbf{I} + \DfDx{\delta q^\star}{q}, \; \mathbf{B} \coloneqq \DfDx{f}{u} = \DfDx{\delta q^\star}{u},
  • The KKT optimality condition of the dynamics SOCP defines equality constraints (including active contact constraints), from which the derivatives of \(\delta q^\star\) w.r.t. \((\mathbf{Q}, b, \phi_i, \mathbf{J}_{\mathrm{n}_i}, \mathbf{J}_{\mathrm{t}_i})\) can be computed using the Implicit Function Theorem (IFT).
  • \(\mathbf{A}, \mathbf{B}\) can then be computed using the gradients from IFT and the chain rule.
  • Standard practice for many differentiable simulators, even though many of them do not find the active constraints by solving convex programs.

Smoothing

  • The contact dynamics \(f(q, u) \in C^0\) has discontinuous gradients, which are bad for planning methods that use gradients.
  • Smoothing with "force at a distance" is a common method to alleviate this problem.

Optimality Condition

\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2 - \frac{1}{\kappa}\sum_i\log{\phi_i(q + \delta q)} \end{aligned}
\Leftrightarrow
\begin{aligned} \underset{\delta q^\mathrm{a}, \delta q^\mathrm{u}}{\min} \; &\frac{1}{2} hK (q + \delta q^\mathrm{a} - u)^2 + \frac{1}{2} \frac{m^\mathrm{u}}{h} (\delta q^\mathrm{u})^2, \; \text{s.t.} \\ &\phi_i (q + \delta q)\geq 0, \forall i. \end{aligned}
\Leftrightarrow

KKT

Original Dynamics

\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ (m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0, \; \forall i, \\ \phi (q + \delta q) \lambda = 0, \forall i. \end{cases}
q^\mathrm{a}
u
K_\mathrm{a}
q^\mathrm{u}
\lambda_1
\lambda_3
\lambda_1
\lambda_2
\phi_2({q})
\phi_1({q})
\phi_3({q})

Smoothed Dynamics

\begin{cases} hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = -\lambda_1 + \lambda_2, \\ (m^\mathrm{u} / h) \delta q^\mathrm{u} = \lambda_1 - \lambda_3, \\ \phi_i(q + \delta q) \geq 0, \; \forall i, \\ \lambda_i \geq 0, \; \forall i, \\ \phi_i (q + \delta q) \lambda = 1 / \kappa, \forall i. \end{cases}

Smoothing, the general case.

\begin{aligned} \underset{\delta q}{\text{min}.} \; &\frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q \;\text{s.t.} \\ & \begin{bmatrix} \phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q \\ \mathbf{J}_{\mathrm{t}_i}\delta q \end{bmatrix} \in \mathcal{K}^\star_i, \; \forall i \in \{1 \cdots n_{\mathrm{c}}\}. \end{aligned}
\underset{\delta q}{\text{min}.} \; \frac{1}{2} {\delta q}^\intercal \mathbf{Q} \delta q + b^\intercal \delta q - \frac{1}{\kappa} \sum_{i=1}^{n_\mathrm{c}} \log \left[ \frac{(\phi_i + \mathbf{J}_{\mathrm{n}_i}\delta q)^2}{\mu_i^2} - (\mathbf{J}_{\mathrm{t}_i}\delta q)^\intercal (\mathbf{J}_{\mathrm{t}_i}\delta q) \right]

Put constraints in a generalized log for \(\mathcal{\kappa}_i^\star\)

\Rightarrow
  • An unconstrained convex program.
  • Can be solved with Newton's method.
  • Also differentiable.

Convex, Quasi-Dynamic, Differentiable Dynamics (an SOCP)

Notation: 

q_+ = f(q, u)

Original dynamics

q_+ = f_\rho(q, u)

Smoothed dynamics

Smoothed gradients enable trajectory optimization for dexterous hands, but...

Task: Turning the ball by 30 degrees.

What about 180 degrees?

Finding this trajectory requires powering through local minima!

Sampling-based Motion Planing is Great at Global Exploration, but...

  • Rapidly-Exploring Random Tree (RRT):   iteratively builds a tree that fills the state space. [1]
  • Dynamic reachability is essential for efficient exploration. [2]
  1. Steven M. LaValle, "Planning Algorithms", Cambridge University Press , 2006.

  2. H.J.T. Suh, J. Deacon, Q. Wang, "A Fast PRM Planner for Car-like Vehicles", self-hosted, 2018.

How do we think about reachability under the contact-dynamics constraint?

A distance metric that reflects reachability

q_+ = f_\rho(q, u),
\newcommand{\DfDx}[2]{\frac{\partial {#1}}{\partial {#2}}} \mathbf{A}_\rho \coloneqq \DfDx{f_\rho}{q}\big|_{q,u}, \\ \mathbf{B}_\rho \coloneqq \DfDx{f_\rho}{u}\big|_{q,u}.
\mathcal{R}_{\rho, \varepsilon}^\mathrm{u} (\bar{q})\coloneqq \left\{\mathbf{B}_\rho^\mathrm{u}(\bar{q},\bar{q}^{\mathrm{a}}) \delta u + f_\rho^\mathrm{u}(\bar{q},\bar{q}^\mathrm{a})\; | \; \|\delta u\|\leq \varepsilon\right\}

Smoothed Contact Dynamics

\(\varepsilon \) sub-level set

Nominal state

un-actauted(objects)

Smoothed

\begin{aligned} d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\ \mu_\rho^\mathrm{u} &\coloneqq f_\rho^\mathrm{u}(\bar{q}, \bar{q}^\mathrm{a}) \end{aligned}

At state \(\bar{q}\), what doe an \(\varepsilon\)-ball in action space get transformed into under the smoothed dynamics?

RRT through contact

Three modifications to the vanilla RRT are made:

  • Nearest node on tree is found using \( d^\mathrm{u}_{\rho}(q; \bar{q}) \).
  • Contact sampling: robot configuration is occasionally reset to one that has a good distance metric, which typically corresponds to grasps.
  • Dynamic-consistent extension: an action is computed using the smooth linearized dynamics to reach \(q\).

Distance Metric

RRT tree for a simple system with contacts

Ablation study: what trees look like after growing a fixed number of nodes.

\begin{aligned} d^\mathrm{u}_{\rho}(q; \bar{q}) &\coloneqq \frac{1}{2} (q^\mathrm{u} - \mu^\mathrm{u}_\rho)^\intercal {\left[\mathbf{B}_{\rho}^{\mathrm{u}} {\mathbf{B}_{\rho}^{\mathrm{u}}}^\intercal\right]}^{-1} (q^\mathrm{u} - \mu^\mathrm{u}_\rho) \\ \mu_\rho^\mathrm{u} &\coloneqq f_\rho^\mathrm{u}(\bar{q}, \bar{q}^\mathrm{a}) \end{aligned}

The modified RRT works well for contact-rich tasks!

Planning wall-clock time (seconds)

Preliminary: stabilizing contact-rich plans

Open-loop

Closed-loop

\begin{aligned} \underset{q^\mathrm{u}_+, u}{\min} &\|q_+^\mathrm{u} - q_+^\mathrm{u, ref}\|_\mathbf{Q} + \|u - u^\mathrm{ref} \|_\mathbf{R}, \text{s.t.}\\ & q^\mathrm{u}_+ = f^\mathrm{u}(q^\mathrm{ref}, u^\mathrm{ref}) + \mathbf{B}_\rho^\mathrm{u} (u - u^\mathrm{ref}) \end{aligned}

Naive QP controller based on smoothed linearization:

Quasi-static dynamical systems: how to fix it.

q^\mathrm{a}
m^\mathrm{a}
u
K
D

A Impedance-controlled 1D robot and an object.

m^\mathrm{u}
q^\mathrm{u}
  1. Pang, Tao, and Russ Tedrake. "A convex quasistatic time-stepping scheme for rigid multibody systems with contact and friction." 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021.
  2. Pang, Tao, and Russ Tedrake. "A robust time-stepping scheme for quasistatic rigid multibody systems." 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

\( x_{+} = f(x, u)\)

  • \(x \coloneqq q \coloneqq  [q^\mathrm{u}, q^\mathrm{a}]\): system configuration.
    • \(q^\mathrm{a}\): actuated, imepdance-controlled robots
    • \(q^\mathrm{u}\): un-actuated objects.
  • \( u \): robot velocities. position command to the impedance-controlled robots. 
\begin{cases} \end{cases}

Contact force and \(q_+\) can now be uniquely* determined from \(u\), even for grasping.

Force-acceleration vs. Impulse-Momentum

q^\mathrm{a}
m^a \ddot{q}^\mathrm{a} + D \dot{q}^\mathrm{a} + K(q^\mathrm{a} - u) = 0
m^\mathrm{a}
u
``\Sigma F = ma"

Force-acceleration

K
D
  1. Stewart, David E. "Rigid-body dynamics with friction and impact." SIAM review 42.1 (2000): 3-39.
``h\Sigma F = m\Delta v"
m^\mathrm{a} (\delta v) + hD v^\mathrm{a} + hK(q^\mathrm{a} + \delta q^\mathrm{a} - u) = 0

Impulse-momentum

0
0
0
0
\underset{\delta q^\mathrm{a}}{\min} \; \frac{1}{2} hK (q^\mathrm{a} + \delta q^\mathrm{a} - u)^2

is the optimality condition of 

But impacts can lead to infinite forces (e.g. Painleve's Paradox) [1].

Time step

Stabilizing contact-rich plans

Open-loop hardware

Open-loop Drake