Discrete-time Control Contraction Metrics (DCCM)

for Quasistatic Planar Pushing

using Smoothed Dynamics

MIT 2.152[J] Nonlinear Control

16 May 2023

By Shao Yuan Chew Chia

Harvard College

Control though Contact

It's important

Challenges:

Non-smooth contact dynamics
Underactuated system

Current Methods

Explicit enumeration of contact modes

Russ Tedrake. Underactuated Robotics: Algorithms for Walking, Running, Swimming, Flying, and Manipulation (Course Notes for MIT 6.832). Downloaded on May 14 from https://underactuated.csail.mit.edu/

Current Methods

Smoothing of contact dynamics

Our Approach

Smoothing of contact dynamics

+

Control Contraction Metrics

Differential version of a Control Lyapunov Function

I. R. Manchester and J.-J. E. Slotine, “Control Contraction Metrics: Convex and Intrinsic Criteria for Nonlinear Feedback Design,” IEEE Transactions on Automatic Control, vol. 62, no. 6, pp. 3046–3053, Jun. 2017, ISSN:1558-2523. DOI: 10.1109/TAC.2017.2668380.

Control Contraction Metrics

Advantages

Certificates of stability and convergence rates
Trajectory independent controllers
Convex synthesis of the controller
Faster online computation of the control law

Problem Setup

Dynamics:

Discrete-time
Analytically smoothed
Nonlinear
Control Affine

x = \begin{bmatrix}b_x & b_y & b_{\theta}& s_x& s_y\end{bmatrix}^\top

x_{k+1} = f(x_k) + g(x_k)u_k

Theory of DCCM

x_{k+1} = f(x_k) + g(x_k)u_k

dynamics

\delta_{x_{k+1}} = A(x_k)\delta_{x_k} + B(x_k)\delta_{u_k}

differential dynamics

A(x_k) = \frac{\partial (f(x_k) + g(x_k)u_k)}{\partial x_k} \in \mathbb{R}^{5 \times 5}

B(x_k) = \frac{\partial (f(x_k) + g(x_k)u_k)}{\partial u_k} \in \mathbb{R}^{5 \times 2}

Jacobians

\delta_{u_k} = K(x_k)\delta_{x_k}

differential state feedback control law

Theory of DCCM

V_k = \delta^\top_{x_k} M_{k} \delta_{x_k}

differential squared distance in the positive definite metric \(M\)

\begin{aligned} V_{k+1} & = \delta^\top_{x_{k+1}} M_{k+1} \delta_{x_{k+1}} \\ & = \delta^\top_{x_k} (A_k + B_k K_k)^\top M_{k+1} (A_k + B_k K_k)\delta_{x_k} \end{aligned}

at the next time step...

V_{k+1} - V_k \leq - \beta V_k < 0 \quad \forall x, \delta_x \in \mathbb{R}^5, \beta \in (0,1]

contraction condition

Theory of DCCM (Online)

L(c) = \int_0^1 \sqrt{\frac{\partial{c(s)}}{\partial{s}} ^\top M(c(s)) \frac{\partial{c(s)}}{\partial{s}}} ds

Riemannian length

E(c) = \int_0^1 L(c)^2 ds

Riemannian energy

\begin{aligned} \gamma(x_0, x_1) &= \argmin_{c} L(c) = \arg \min_{c} E(c) \end{aligned}

geodesic

u_k = u^*_k + \int_0^1 K(\gamma(s))\frac{\partial{\gamma(s)}}{\partial{s}}

tracking controller

Methods: DCCM Synthesis

Sums of Squares (SOS) Programming

V_{k+1} - V_k \leq - \beta V_k < 0

contraction condition

\Rightarrow (A_k + B_k K_k)^\top M_{k+1} (A_k + B_k K_k) - (1 - \beta) M_k < 0

where \(W := M^{-1}\) and \(L := KW\)

\Rightarrow \begin{bmatrix} W_{k+1} & A_k + B_k L_k \\ (A_k + B_k L_k)^\top & (1 - \beta) W_k \end{bmatrix} > 0

Methods: DCCM Synthesis

Sums of Squares (SOS) Programming

\begin{aligned} \min_{l_c, w_c, r} \quad& r \\ s.t. \quad& \forall k, w^\top \Omega w - r w^\top w \in \Sigma(x_k, u_k, w) \\ & r \geq 0.1 \end{aligned}

\Omega = \begin{bmatrix} W_{k+1} & A_k + B_k L_k \\ (A_k + B_k L_k)^\top & (1 - \beta) W_k \end{bmatrix}

is SOS

relaxation slack variable

coefficients of \(L\) and \(W\)

enforced over samples

Methods: DCCM Synthesis

Sums of Squares (SOS) Programming

L_k = \begin{bmatrix} L_{11_k} & \cdots & L_{15_k} \\ L_{21_k} & \cdots & L_{25_k} \\ \end{bmatrix}

W_k = \begin{bmatrix} W_{11_k} & \cdots & W_{15_k} \\ \vdots & \ddots & \vdots \\ W_{15_k} & \cdots & W_{55_k} \\ \end{bmatrix}

matrix of polynomials

W.._{k} = w.._c v(x_k)

L.._k = l.._c v(x_k)

polynomial expression

v(x_k) = [x^4_{k_4}, x_{k_3}x^3_{k_4}, x^2_{k_3}x^2_{k_4}, \cdots ,x_{k_1}, x_{k_0}, 1]

monomial basis

Methods: DCCM Synthesis

Sampling strategy

Methods: Online Computation

Finding the geodesic

\forall i, \Delta s[i] > 0, \quad \sum_{i=0}^{N-1} \Delta s[i] = 1

\begin{aligned} \bar{\gamma}(x^*_k, x_k) = \argmin_{ \substack{ x[\cdot], \Delta x_{s}[\cdot],\\ \Delta s[\cdot]}} & \sum_{i=0}^{N-1} \Delta s[i] \Delta x_s[i]^\top M(x[i]) \Delta x_s[i] \\ \end{aligned}

geodesic energy

\begin{aligned} s.t. \quad x[0] = x^*_k, \quad x[N] = x_k \\ \end{aligned}

start and end of geodesic

\forall i, x[i+1] = x[i] + \Delta x_{s}[i]\Delta s[i]

continuity

Methods: Online Computations

Finding the geodesic

\forall i, \Delta s[i] > 0, \quad \sum_{i=0}^{N-1} \Delta s[i] = 1

\begin{aligned} \bar{\gamma}(x^*_k, x_k) = \argmin_{ \substack{ x[\cdot], \Delta x_{s}[\cdot],\\ \Delta s[\cdot], m[\cdot], y[\cdot]}} & \sum_{i=0}^{N-1} y[i] \end{aligned}

\begin{aligned} s.t. \quad x[0] = x^*_k, \quad x[N] = x_k \\ \end{aligned}

\forall i, x[i+1] = x[i] + \Delta x_{s}[i]\Delta s[i]

geodesic energy

\forall i, y[i] \geq \Delta s[i] \Delta x_s[i]^\top M(m[i]) \Delta x_s[i] \\

+ \Delta s[i]^2

even out length of segments

\( M = W^{-1}\)

\forall i, M(m[i]) W(x[i]) = I

Methods: Online Computations

Tracking controller

\begin{aligned} u_k = u^*_k + \sum_{i=0}^{N-1} \Delta s[i] K(x[i]) \Delta x_s[i] \end{aligned}

= u^*_k + \sum_{i=0}^{N-1} \Delta s[i] L(x[i]) W^{-1}(x[i]) \Delta x_s

Results

Overview

Results

Controller performance

Results

Effect of number of samples on performance

Results

Unexplained issues

Discretization of the geodesic
- Optimization program failed when \(N > 1\)
Choosing \(\beta\)
- Synthesizing DCCM with \(\beta > 0.3\) failed
- DCCM with \(\beta = 0.3\) did not stabilize the system while \(\beta = 0.1\) did

Results

Computation time

Task	Parameters	Time
Synthesis	Deg 4, 500 Samples	18 min
Synthesis	Deg 6, 500 Samples	3 hr 20 min
Synthesis	Deg 4, 2000 Samples	1 hr
Geodesic	Deg 4, 1 Segment	1.54 s
Geodesic	Deg 6, 1 Segment	3.81 s

Linux machine with 31.3GB of RAM, an with an Intel Core i7-6700 CPU @ 3.40GHz x 8 processor.

Conclusion and Future Work

Success:

Synthesized stabilizing DCCM with highly smoothed dynamics for planar pushing

Yet to be achieved:

Certificates of stability and convergence rates
- Error due to sparse sampling
- Error due to exact vs smoothed dynamics
Trajectory independent controller
Synthesis of controller for contact dynamics with less smoothing
- Learn the metric [13], [14]
Faster computation of the control law
- Warm starting techniques
- Pseudospectral approaches [15]

Other possible directions: stabilizing to submanifold [8]

References

[1] T. Pang, H. J. T. Suh, L. Yang, and R. Tedrake. (Feb. 27,
2023). Global Planning for Contact-Rich Manipulation
via Local Smoothing of Quasi-dynamic Contact Mod-
els. Comment: The first two authors contributed equally
to this work. arXiv: 2206 . 10787 [cs], [Online].
Available: http : / / arxiv. org / abs / 2206 . 10787 (visited
on 03/15/2023), preprint.

[2] D. E. Stewart and M. Anitescu, “Optimal control of
systems with discontinuous differential equations,” Nu-
merische Mathematik, vol. 114, no. 4, pp. 653–695,
Feb. 1, 2010, ISSN: 0945-3245. DOI: 10.1007/s00211-
009 - 0262 - 2. [Online]. Available: https : / / doi . org / 10 .
1007/s00211-009-0262-2 (visited on 05/12/2023).

[3] F. R. Hogan and A. Rodriguez. (Nov. 24, 2016). Feed-
back Control of the Pusher-Slider System: A Story of
Hybrid and Underactuated Contact Dynamics. arXiv:
1611.08268 [cs], [Online]. Available: http://arxiv.org/
abs/1611.08268 (visited on 05/12/2023), preprint.

[4] J.-P. Sleiman, J. Carius, R. Grandia, M. Wer-
melinger, and M. Hutter, “Contact-Implicit Trajectory
Optimization for Dynamic Object Manipulation,” in
2019 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), Comment: 8 Pages, 10
Figures. Submitted to the International Conference on
Intelligent Robots and Systems (IROS), 2019, Nov.
2019, pp. 6814–6821. DOI: 10 . 1109 / IROS40897 .
2019 . 8968194. arXiv: 2103 . 01104 [cs]. [Online].
Available: http : / / arxiv. org / abs / 2103 . 01104 (visited
on 03/17/2023).

[5] K. Nakatsuru, W. Wan, and K. Harada. (Feb. 25, 2023).
Implicit Contact-Rich Manipulation Planning for a Ma-
nipulator with Insufficient Payload. arXiv: 2302.13212
[cs], [Online]. Available: http://arxiv.org/abs/2302.
13212 (visited on 03/17/2023), preprint.

[6] M. Posa, C. Cantu, and R. Tedrake, “A direct method
for trajectory optimization of rigid bodies through
contact,” International Journal of Robotics Research,
vol. 33, no. 1, pp. 69–81, Jan. 1, 2014, ISSN: 0278-3649.
DOI: 10.1177/0278364913506757. [Online]. Available:
https://doi.org/10.1177/0278364913506757 (visited on
03/17/2023).

[7] R. Tedrake, I. R. Manchester, M. Tobenkin, and
J. W. Roberts, “LQR-trees: Feedback Motion Plan-
ning via Sums-of-Squares Verification,” The Interna-
tional Journal of Robotics Research, vol. 29, no. 8,
pp. 1038–1052, Jul. 1, 2010, ISSN: 0278-3649. DOI:
10.1177/0278364910369189. [Online]. Available: https:
/ / doi . org / 10 . 1177 / 0278364910369189 (visited on
05/13/2023).

[8] I. R. Manchester and J.-J. E. Slotine, “Control Contrac-
tion Metrics: Convex and Intrinsic Criteria for Nonlin-
ear Feedback Design,” IEEE Transactions on Automatic
Control, vol. 62, no. 6, pp. 3046–3053, Jun. 2017, ISSN:
1558-2523. DOI: 10.1109/TAC.2017.2668380.

References

[9] I. R. Manchester, J. Z. Tang, and J.-J. E. Slotine, “Unify-
ing Robot Trajectory Tracking with Control Contraction
Metrics,” in Robotics Research: Volume 2, ser. Springer
Proceedings in Advanced Robotics, A. Bicchi and W.
Burgard, Eds., Cham: Springer International Publishing,
2018, pp. 403–418, ISBN: 978-3-319-60916-4. DOI: 10.
1007 / 978 - 3 - 319 - 60916 - 4 23. [Online]. Available:
https://doi.org/10.1007/978-3-319-60916-4 23 (visited
on 05/05/2023).

[10] S. Singh, A. Majumdar, J.-J. Slotine, and M. Pavone,
“Robust Online Motion Planning via Contraction The-
ory and Convex Optimization,” May 29, 2017. DOI: 10.
1109/ICRA.2017.7989693.

[11] L. Wei, R. Mccloy, and J. Bao, “Control Contraction
Metric Synthesis for Discrete-time Nonlinear Systems,”
IFAC-PapersOnLine, 16th IFAC Symposium on Ad-
vanced Control of Chemical Processes ADCHEM 2021,
vol. 54, no. 3, pp. 661–666, Jan. 1, 2021, ISSN: 2405-
8963. DOI: 10 . 1016 / j . ifacol . 2021 . 08 . 317. [Online].
Available: https : / / www . sciencedirect . com / science /
article/pii/S2405896321010910 (visited on 04/16/2023).

[12] Russ Tedrake and the Drake Development Team. (2019). Drake:
Model-Based Design and Verification for Robotics,
[Online]. Available: https://drake.mit.edu/ (visited on
05/10/2023).

[13] S. Singh, V. Sindhwani, J.-J. E. Slotine, and M. Pavone.
(Jul. 31, 2018). Learning Stabilizable Dynamical Sys-
tems via Control Contraction Metrics, arXiv.org, [On-
line]. Available: https : / / arxiv. org / abs / 1808 . 00113v2
(visited on 05/04/2023).

[14] G. Chou, N. Ozay, and D. Berenson, “Model Er-
ror Propagation via Learned Contraction Metrics for
Safe Feedback Motion Planning of Unknown Sys-
tems,” Dec. 14, 2021, pp. 3576–3583. DOI: 10.1109/
CDC45484.2021.9683354.

[15] K. Leung and I. R. Manchester. (Nov. 4, 2017). Non-
linear Stabilization via Control Contraction Metrics:
A Pseudospectral Approach for Computing Geodesics.
arXiv: 1607 . 04340 [cs], [Online]. Available: http :
/ / arxiv. org / abs / 1607 . 04340 (visited on 05/06/2023),
preprint.

Images:

https://news.clearancejobs.com/wp-content/uploads/2021/08/1150x732-2021-08-23T065258.010.jpg
https://cdn.shopify.com/s/files/1/0091/4056/8111/products/PRUB007_Medium_Moving_Box_18__x_18__x_16_2000x.jpg?v=1565898544