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
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
Discrete-time Control Contraction Metrics (DCCM) for Quasistatic Planar Pushing using Smoothed Dynamics
By shao_yuan
