Robots must make and break contact with the environment to perform useful tasks, but planning and control through contact remains a formidable challenge. In this work, we achieve real-time contact-implicit model predictive control with a surprisingly simple method: inverse dynamics trajectory optimization. While trajectory optimization with inverse dynamics is not new, we introduce a series of incremental innovations that collectively enable fast model predictive control on a variety of challenging manipulation and locomotion tasks. We implement these innovations in an open-source solver and present simulation examples to support the effectiveness of the proposed approach. Additionally, we demonstrate contact-implicit model predictive control on hardware at over 100 Hz for a 20-degree-of-freedom bi-manual manipulation task. Video and code are available at https://idto.github.io.
AydinogluAWeiAPosaM (2023) Consensus complementarity control for multi-contact mpc. arXiv preprint arXiv:2304.11259.
2.
BenkertKFischerR (2007) An efficient implementation of the thomas-algorithm for block penta-diagonal systems on vector computers. In: International Conference on Computational Science. Springer, 144–151.
3.
CariusJRanftlRKoltunV, et al. (2018) Trajectory optimization with implicit hard contacts. IEEE Robotics and Automation Letters3(4): 3316–3323.
4.
CarpentierJMansardN (2018) Analytical derivatives of rigid body dynamics algorithms. In: Robotics: Science and Systems (RSS 2018).
5.
CarpentierJSaurelGBuondonnoG, et al. (2019) The pinocchio c++ library: a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives. In: 2019 IEEE/SICE International Symposium on System Integration (SII). IEEE, 614–619.
6.
CastroAMQuAKuppuswamyN, et al. (2020) A transition-aware method for the simulation of compliant contact with regularized friction. IEEE Robotics and Automation Letters5(2): 1859–1866.
7.
CastroAMPermenterFNHanX (2022) An unconstrained convex formulation of compliant contact. IEEE Transactions on Robotics39(2): 1301–1320.
ChatzinikolaidisILiZ (2021) Trajectory optimization of contact-rich motions using implicit differential dynamic programming. IEEE Robotics and Automation Letters6(2): 2626–2633.
10.
ChiCFengSDuY, et al. (2023) Diffusion policy: visuomotor policy learning via action diffusion. arXiv preprint arXiv:2303.04137.
11.
Cleac’hSLHowellTSchwagerM, et al. (2023) Fast contact-implicit model-predictive control. arXiv preprint arXiv:2107.05616.
12.
CuiLDaiJ (2010) Geometric kinematics of rigid bodies with point contact. In: Advances in Robot Kinematics: Motion in Man and Machine: Motion in Man and Machine. Springer, 429–436.
13.
DiehlMBockHGSchlöderJP (2005) A real-time iteration scheme for nonlinear optimization in optimal feedback control. SIAM Journal on Control and Optimization43(5): 1714–1736.
14.
DietzCNurkanovićAAlbrechtS, et al. (2024) High accuracy numerical optimal control for rigid bodies with patch contacts through equivalent contact points–extended version. arXiv preprint arXiv:2403.13931.
15.
ElandtRDrumwrightEShermanM, et al. (2019) A pressure field model for fast, robust approximation of net contact force and moment between nominally rigid objects. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 8238–8245.
16.
ErezTTodorovE (2012) Trajectory optimization for domains with contacts using inverse dynamics. In: 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 4914–4919.
17.
FerrolhoHIvanVMerktW, et al. (2021) Inverse dynamics vs. forward dynamics in direct transcription formulations for trajectory optimization. In: 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 12752–12758.
18.
GiftthalerMNeunertMStäubleM, et al. (2018) A family of iterative gauss-Newton shooting methods for nonlinear optimal control. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 1–9.
19.
GilbertEGJohnsonDWKeerthiSS (1988) A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal of Robotics and Automation4(2): 193–203.
20.
HowellTGileadiNTunyasuvunakoolS, et al. (2022a) Predictive sampling: real-time behaviour synthesis with mujoco. arXiv preprint arXiv:2212.00541.
21.
HowellTALeC’h SKolterJZ, et al. (2022b) Dojo: a differentiable simulator for robotics. arXiv preprint arXiv:2203.00806 9.
22.
HowellTALeC’h SSinghS, et al. (2022c) Trajectory optimization with optimization-based dynamics. IEEE Robotics and Automation Letters7(3): 6750–6757.
23.
HuntKCrossleyF (1975) Coefficient of restitution interpreted as damping in vibroimpact. Journal of Applied Mechanics42(2): 440–445.
24.
KatzBDi CarloJKimS (2019) Mini cheetah: a platform for pushing the limits of dynamic quadruped control. In: 2019 International Conference on Robotics and Automation (ICRA). IEEE, 6295–6301.
25.
KhazoomCHongSChignoliM, et al. (2024) Tailoring solution accuracy for fast whole-body model predictive control of legged robots. IEEE Robotics and Automation Letters.
26.
KimGKangDKimJH, et al. (2022) Contact-implicit differential dynamic programming for model predictive control with relaxed complementarity constraints. In: 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 11978–11985.
27.
KimGKangDKimJH, et al. (2023) Contact-implicit mpc: controlling diverse quadruped motions without pre-planned contact modes or trajectories. arXiv preprint arXiv:2312.08961.
28.
KongNJLiCJohnsonAM (2022) Hybrid iLQR model predictive control for contact implicit stabilization on legged robots. arXiv preprint arXiv:2207.04591.
29.
Le LidecQSchrammFMontautL, et al. (2024) Leveraging randomized smoothing for optimal control of nonsmooth dynamical systems. Nonlinear Analysis: Hybrid Systems52: 101468.
30.
LiWTodorovE (2004) Iterative linear quadratic regulator design for nonlinear biological movement systems. In: ICINCO (1). Citeseer, 222–229.
31.
ManchesterZDoshiNWoodRJ, et al. (2019) Contact-implicit trajectory optimization using variational integrators. The International Journal of Robotics Research38(12-13): 1463–1476.
32.
MarcucciTUmenbergerJParriloPA, et al. (2021) Shortest paths in graphs of convex sets. arXiv preprint arXiv:2101.11565.
33.
MasterjohnJGuoyDShepherdJ, et al. (2022) Velocity level approximation of pressure field contact patches. IEEE Robotics and Automation Letters7(4): 11593–11600.
34.
MayneD (1966) A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems. International Journal of Control3(1): 85–95.
35.
MordatchIPopovićZTodorovE (2012a) Contact-invariant optimization for hand manipulation. In: Proceedings of the ACM SIGGRAPH/Eurographics Symposium on Computer Animation, 137–144.
36.
MordatchITodorovEPopovićZ (2012b) Discovery of complex behaviors through contact-invariant optimization. ACM Transactions on Graphics31(4): 1–8.
37.
MoréJJ (2006) The levenberg-marquardt algorithm: implementation and theory. In: Numerical Analysis: Proceedings of the Biennial Conference Held at Dundee, Dundee, June 28–July 1, 1977. Springer, 105–116.
38.
MouraJStouraitisTVijayakumarS (2022) Non-prehensile planar manipulation via trajectory optimization with complementarity constraints. In: 2022 International Conference on Robotics and Automation (ICRA). IEEE, 970–976.
39.
NatarajanRJohnstonGLSimaanN, et al. (2023) Torque-limited manipulation planning through contact by interleaving graph search and trajectory optimization. In: 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE.
40.
NeunertMFarshidianFWinklerAW, et al. (2017) Trajectory optimization through contacts and automatic gait discovery for quadrupeds. IEEE Robotics and Automation Letters2(3): 1502–1509.
41.
NeunertMStäubleMGiftthalerM, et al. (2018) Whole-body nonlinear model predictive control through contacts for quadrupeds. IEEE Robotics and Automation Letters3(3): 1458–1465.
42.
NocedalJWrightSJ (2006) Numerical Optimization. 2nd edition. New York: Springer.
43.
ÖnolAÖLongPPadırT (2019) Contact-implicit trajectory optimization based on a variable smooth contact model and successive convexification. In: 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2447–2453.
44.
ÖnolAÖCorcodelRLongP, et al. (2020) Tuning-free contact-implicit trajectory optimization. In: 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 1183–1189.
45.
PangTSuhHTYangL, et al. (2023) Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models. In: IEEE Transactions on Robotics. IEEE.
46.
PatelAShieldSLKaziS, et al. (2019) Contact-implicit trajectory optimization using orthogonal collocation. IEEE Robotics and Automation Letters4(2): 2242–2249.
47.
PosaMTedrakeR (2013) Direct trajectory optimization of rigid body dynamical systems through contact. In: Algorithmic Foundations of Robotics. Springer, 527–542.
48.
PosaMKuindersmaSTedrakeR (2016) Optimization and stabilization of trajectories for constrained dynamical systems. In: 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 1366–1373.
49.
SinghSRussellRPWensingPM (2022) Efficient analytical derivatives of rigid-body dynamics using spatial vector algebra. IEEE Robotics and Automation Letters7(2): 1776–1783.
50.
SuhHJTPangTTedrakeR (2022) Bundled gradients through contact via randomized smoothing. IEEE Robotics and Automation Letters7(2): 4000–4007.
51.
TassaYErezTTodorovE (2012) Synthesis and stabilization of complex behaviors through online trajectory optimization. In: 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 4906–4913.
52.
TedrakeRthe Drake Development Team (2019) Drake: model-based design and verification for robotics. https://drake.mit.edu
TodorovE (2019b) Optico: a framework for model-based optimization with mujoco physics. Invited Talk, Neural Information Processing Systems. https://slideslive.com/38922729/
55.
WangMÖnolAÖLongP, et al. (2023) Contact-implicit planning and control for non-prehensile manipulation using state-triggered constraints. In: Robotics Research. Springer, 189–204.
56.
WensingPMPosaMHuY, et al. (2022) Optimization-based control for dynamic legged robots. arXiv preprint arXiv:2211.11644.
57.
WinklerAWBellicosoCDHutterM, et al. (2018) Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robotics and Automation Letters3(3): 1560–1567.
Supplementary Material
Please find the following supplemental material available below.
For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.
For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.
