A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation

In this letter, we propose a whole-body planning framework that unifies dynamic locomotion and manipulation tasks by formulating a single multi-contact optimal control problem. We model the hybrid nature of a generic multi-limbed mobile manipulator as a switched system, and introduce a set of constraints that can encode any pre-defined gait sequence or manipulation schedule in the formulation. Since the system is designed to actively manipulate its environment, the equations of motion are composed by augmenting the robot's centroidal dynamics with the manipulated-object dynamics. This allows us to describe any high-level task in the same cost/constraint function. The resulting planning framework could be solved on the robot's onboard computer in real-time within a model predictive control scheme. This is demonstrated in a set of real hardware experiments done in free-motion, such as base or end-effector pose tracking, and while pushing/pulling a heavy resistive door. Robustness against model mismatches and external disturbances is also verified during these test cases.

[1]  D K Smith,et al.  Numerical Optimization , 2001, J. Oper. Res. Soc..

[2]  Jonas Buchli,et al.  An efficient optimal planning and control framework for quadrupedal locomotion , 2016, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[3]  Pierre-Brice Wieber,et al.  Hierarchical quadratic programming: Fast online humanoid-robot motion generation , 2014, Int. J. Robotics Res..

[4]  Edo Jelavic,et al.  Real-time motion planning of legged robots: A model predictive control approach , 2017, 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids).

[5]  Guy Bessonnet,et al.  Forces acting on a biped robot. Center of pressure-zero moment point , 2004, IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and Humans.

[6]  Sangbae Kim,et al.  Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[7]  Ruben Grandia,et al.  Feedback MPC for Torque-Controlled Legged Robots , 2019, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[8]  Russ Tedrake,et al.  Whole-body motion planning with centroidal dynamics and full kinematics , 2014, 2014 IEEE-RAS International Conference on Humanoid Robots.

[9]  Olivier Stasse,et al.  Optimal control for whole-body motion generation using center-of-mass dynamics for predefined multi-contact configurations , 2015, 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids).

[10]  Claudio Semini,et al.  MPC-based Controller with Terrain Insight for Dynamic Legged Locomotion , 2020, 2020 IEEE International Conference on Robotics and Automation (ICRA).

[11]  Olivier Stasse,et al.  The Pinocchio C++ library : A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives , 2019, 2019 IEEE/SICE International Symposium on System Integration (SII).

[12]  Marco Hutter,et al.  Constraint Handling in Continuous-Time DDP-Based Model Predictive Control , 2021, 2021 IEEE International Conference on Robotics and Automation (ICRA).

[13]  Russ Tedrake,et al.  Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact , 2012, WAFR.

[14]  Marco Hutter,et al.  ALMA - Articulated Locomotion and Manipulation for a Torque-Controllable Robot , 2019, 2019 International Conference on Robotics and Automation (ICRA).

[15]  Olivier Stasse,et al.  Whole-body model-predictive control applied to the HRP-2 humanoid , 2015, 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[16]  Pierre-Brice Wieber,et al.  Kinematic Control of Redundant Manipulators: Generalizing the Task-Priority Framework to Inequality Task , 2011, IEEE Transactions on Robotics.

[17]  E. Todorov,et al.  A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems , 2005, Proceedings of the 2005, American Control Conference, 2005..

[18]  Marco Hutter,et al.  Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization , 2018, IEEE Robotics and Automation Letters.

[19]  Scott Kuindersma,et al.  An efficiently solvable quadratic program for stabilizing dynamic locomotion , 2013, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[20]  Carlos Mastalli,et al.  Passive Whole-Body Control for Quadruped Robots: Experimental Validation Over Challenging Terrain , 2018, IEEE Robotics and Automation Letters.

[21]  Peter Fankhauser,et al.  Dynamic locomotion and whole-body control for quadrupedal robots , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[22]  David Q. Mayne,et al.  Differential dynamic programming , 1972, The Mathematical Gazette.

[23]  Nicolas Mansard,et al.  Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control , 2020, 2020 IEEE International Conference on Robotics and Automation (ICRA).

[24]  Peter Fankhauser,et al.  Perception-less terrain adaptation through whole body control and hierarchical optimization , 2016, 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids).

[25]  David E. Orin,et al.  Centroidal dynamics of a humanoid robot , 2013, Auton. Robots.

[26]  MORITZ DIEHL,et al.  A Real-Time Iteration Scheme for Nonlinear Optimization in Optimal Feedback Control , 2005, SIAM J. Control. Optim..

[27]  Pierre-Brice Wieber,et al.  Holonomy and Nonholonomy in the Dynamics of Articulated Motion , 2006 .

[28]  Marco Hutter,et al.  Whole-Body Nonlinear Model Predictive Control Through Contacts for Quadrupeds , 2017, IEEE Robotics and Automation Letters.

[29]  Oussama Khatib,et al.  A unified approach for motion and force control of robot manipulators: The operational space formulation , 1987, IEEE J. Robotics Autom..