Skip to main content


Log in

Optimization-based dynamic motion planning and control for quadruped robots

  • Original Paper
  • Published:
Nonlinear Dynamics Aims and scope Submit manuscript


Fast trajectory planning and control frameworks improve locomotion robustness against disturbances and uncertainties. In this paper, dynamic motions are optimized under the constraint of a decoupled spring-loaded inverted pendulum model. Subsequently, a hierarchical Quadratic Programming whole-body controller is employed to execute the planned trajectories while ensuring compliance with all physical feasibility constraints. Both the motion planner and the whole-body controller operate within the same high-frequency control loop. Furthermore, the unified whole-body controller governs all gait phases, including flight phases. The proposed algorithms are evaluated through simulation and real experiments, showcasing dynamic gaits such as hopping, twist jumping, and trotting in challenging environments. The algorithms demonstrate resilience against external disturbances and environmental uncertainties.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13
Fig. 14
Fig. 15
Fig. 16

Similar content being viewed by others

Data availibility

Data sharing not applicable to this article as no datasets were generated or analysed during the current study.


  1. Yang, C., Yuan, K., Zhu, Q., Yu, W., Li, Z.: Multi-expert learning of adaptive legged locomotion. Sci. Robot. 5, eabb2174 (2020)

    Article  Google Scholar 

  2. Lee, J., Hwangbo, J., Wellhausen, L., Koltun, V., Hutter, M.: Learning quadrupedal locomotion over challenging terrain. Sci. Robot. 5, eabc5986 (2020)

    Article  Google Scholar 

  3. Hwangbo, J., et al.: Learning agile and dynamic motor skills for legged robots. Sci. Robot. 4, eaau5872 (2019)

    Article  Google Scholar 

  4. Winkler, A.W., Bellicoso, C.D., Hutter, M., Buchli, J.: Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robot. Autom. Lett. 3, 1560–1567 (2018)

    Article  Google Scholar 

  5. Sleiman, J.-P., Farshidian, F., Minniti, M.V., Hutter, M.: A unified MPC framework for whole-body dynamic locomotion and manipulation. IEEE Robot. Autom. Lett. 6, 4688–4695 (2021)

    Article  Google Scholar 

  6. Mastalli, C., et al.: Crocoddyl: an efficient and versatile framework for multi-contact optimal control, pp. 2536–2542. IEEE (2020)

  7. Cebe, O., et al.: Online dynamic trajectory optimization and control for a quadruped robot, pp. 12773–12779 (2021)

  8. Kim, D., Di Carlo, J., Katz, B., Bledt, G., Kim, S.: Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control (2019). arXiv preprint arXiv:1909.06586

  9. Kajita, S., et al.: Biped walking pattern generation by using preview control of zero-moment point, vol. 2, pp. 1620–1626. IEEE (2003)

  10. Mordatch, I., de Lasa, M., Hertzmann, A.: Robust physics-based locomotion using low-dimensional planning. ACM Trans. Graph 29(4) (2010).

  11. Apgar, T., Clary, P., Green, K., Fern, A., Hurst, J.W.: Fast online trajectory optimization for the bipedal robot Cassie. In: Robotics: Science and Systems, vol. 101, p. 14. Pittsburgh, Pennsylvania, USA (2018)

  12. Xin, G., et al.: Robust footstep planning and LQR control for dynamic quadrupedal locomotion. IEEE Robot. Autom. Lett. 6, 4488–4495 (2021)

    Article  Google Scholar 

  13. Bloesch, M., et al.: State estimation for legged robots-consistent fusion of leg kinematics and IMU. Robotics 17, 17–24 (2013)

    Article  Google Scholar 

  14. Pratt, J., Chew, C.-M., Torres, A., Dilworth, P., Pratt, G.: Virtual model control: an intuitive approach for bipedal locomotion. Int. J. Robot. Res. 20, 129–143 (2001)

    Article  Google Scholar 

  15. Boaventura, T., et al.: Dynamic torque control of a hydraulic quadruped robot, pp. 1889–1894. IEEE (2012)

  16. Gehring, C., et al.: Control of dynamic gaits for a quadrupedal robot, pp. 3287–3292. IEEE (2013)

  17. Feng, S., Whitman, E., Xinjilefu, X., Atkeson, C.G.: Optimization based full body control for the atlas robot. In: IEEE-RAS International Conference on Humanoid Robots, 2015-Feb, pp. 120–127 (2015)

  18. Di Carlo, J., Wensing, P.M., Katz, B., Bledt, G., Kim, S.: Dynamic locomotion in the MIT cheetah 3 through convex model-predictive control, pp. 1–9. IEEE (2018)

  19. Xin, S., Orsolino, R., Tsagarakis, N.: Online relative footstep optimization for legged robots dynamic walking using discrete-time model predictive control. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 513–520. IEEE (2019)

  20. De Lasa, M., Mordatch, I., Hertzmann, A.: Feature-based locomotion controllers, vol. 29, 131. ACM (2010)

  21. Del Prete, A., Nori, F., Metta, G., Natale, L.: Prioritized motion-force control of constrained fully-actuated robots “task space inverse dynamics’’. Robot. Auton. Syst. 63, 150–157 (2015)

    Article  Google Scholar 

  22. Herzog, A., et al.: Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid. Auton. Robot. 40, 473–491 (2016)

    Article  Google Scholar 

  23. Dario Bellicoso, C., Gehring, C., Hwangbo, J., Fankhauser, P., Hutter, M.: Perception-less terrain adaptation through whole body control and hierarchical optimization. In: IEEE-RAS International Conference on Humanoid Robots, pp. 558–564 (2016)

  24. Kajita, S., Matsumoto, O., Saigo, M.: Real-time 3d walking pattern generation for a biped robot with telescopic legs, vol. 3, pp. 2299–2306. IEEE (2001)

  25. Kajita, S., Hirukawa, H., Harada, K., Yokoi, K.: Introduction to humanoid robotics, vol. 101. Springer (2014)

  26. Dasgupta, A., Nakamura, Y.: Making feasible walking motion of humanoid robots from human motion capture data. In: Proceedings 1999 IEEE international conference on robotics and automation (Cat. No. 99CH36288C), vol. 2, pp. 1044–1049. IEEE (1999)

  27. Wensing, P.M., Orin, D.E.: Improved computation of the humanoid centroidal dynamics and application for whole-body control. Int. J. Humanoid Rob. 13, 1550039 (2016)

    Article  Google Scholar 

  28. Orin, D.E., Goswami, A., Lee, S.-H.: Centroidal dynamics of a humanoid robot. Auton. Robot. 35, 161–176 (2013)

    Article  Google Scholar 

  29. Mistry, M., Buchli, J., Schaal, S.: Inverse dynamics control of floating base systems using orthogonal decomposition. In: Proceedings—IEEE International Conference on Robotics and Automation, pp. 3406–3412 (2010)

  30. Wächter, A., Biegler, L.T.: On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Math. Program. 106, 25–57 (2006)

    Article  MathSciNet  Google Scholar 

  31. Wang, K., et al.: A unified model with inertia shaping for highly dynamic jumps of legged robots (2021). arXiv preprint arXiv:2109.04581

Download references


The authors would like to thank Ke Wang from Imperial College London for the trajectory generation of the 90\(^\circ \) twist jumping, and Dr. Wolfgang Merkt from the University of Oxford for the experimental tests of the proposed algorithms on the ANYmal Boxy robot at Oxford.


This work was supported in part by Grants EPSRC UK RAI Hubs NCNR (EP/R02572X/1), ORCA (EP/R026173/1) and EU Horizon 2020 THING (ICT-27-2017 780883), the Liaoning and Guangdong Basic and Applied Basic Research Foundation [2023JH2/101600033] [2022A1515140156], the Fundamental Research Funds for the Central Universities [82232025], the State Key Laboratory of Robotics and Systems funding [SKLRS-2023-KF-16]. Dalian Science and Technology Innovation Funding (2023JJ12GX017).

Author information

Authors and Affiliations



All authors contributed to the study conception and design. Material preparation, data collection and analysis were performed by GX. The first draft of the manuscript was written by GX and all authors commented on previous versions of the manuscript. All authors read and approved the final manuscript.

Corresponding author

Correspondence to Guiyang Xin.

Ethics declarations

Conflict of interests

The authors have no relevant financial or non-financial interests to disclose.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.



1.1 Dimension reduction

Equation (20) can be directly split into two equations

$$\begin{aligned}{} & {} \textbf{M}_f\ddot{\textbf{q}}+\textbf{h}_f=\textbf{J}_{c,f}^\top \varvec{\lambda } \end{aligned}$$
$$\begin{aligned}{} & {} \textbf{M}_a\ddot{\textbf{q}}+\textbf{h}_a=\varvec{\tau }+\textbf{J}_{c,a}^\top \varvec{\lambda } \end{aligned}$$

where \(\textbf{M}_f\in \mathbb {R}^{6\times (6+n)}\), \(\textbf{h}_f \in \mathbb {R}^6\) and \(\textbf{J}_{c,f} \in \mathbb {R}^{3\,m \times 6}\) denote the floating base components of the dynamic matrices while \(\textbf{M}_a\in \mathbb {R}^{n\times (6+n)}\), \(\textbf{h}_a \in \mathbb {R}^n\) and \(\textbf{J}_{c,a} \in \mathbb {R}^{3\,m \times n}\) represent the actuated components. Apparently \(\varvec{\tau }\) is determined by Eq. (39) as follows

$$\begin{aligned} \varvec{\tau }=\textbf{M}_a\ddot{\textbf{q}}+\textbf{h}_a-\textbf{J}_{c,a}^\top \varvec{\lambda } \end{aligned}$$

Therefore, the decision variable can be reduced to \(\textbf{y}=\begin{bmatrix}\ddot{\textbf{q}}^\top&\varvec{\lambda }^\top \end{bmatrix}^\top \), and the physical feasibility constraint Eq. (28) becomes

$$\begin{aligned} \begin{bmatrix} -\textbf{h}_f \\ dot{\textbf{J}}_{c}\dot{\textbf{q}} \\ -\mathbf {\infty } \\ \varvec{\tau }_{\textrm{min}}-\textbf{h}_a\end{bmatrix} \le \begin{bmatrix}\textbf{M}_f &{} -\textbf{J}_{c,f}^\top \\ \textbf{J}_c &{} \textbf{0} \\ \textbf{0} &{} \textbf{C}_{\textrm{in}} \\ \textbf{M}_a &{} -\textbf{J}_{c,a}^\top \end{bmatrix}\textbf{y} \le \begin{bmatrix} -\textbf{h}_f \\ dot{\textbf{J}}_c\dot{\textbf{q}} \\ \textbf{0} \\ \varvec{\tau }_{\textrm{max}}-\textbf{h}_a\end{bmatrix} \end{aligned}$$

The dimensions of the cost functions also need to be modified using the reduced decision variable.

Rights and permissions

Springer Nature or its licensor (e.g. a society or other partner) holds exclusive rights to this article under a publishing agreement with the author(s) or other rightsholder(s); author self-archiving of the accepted manuscript version of this article is solely governed by the terms of such publishing agreement and applicable law.

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Xin, G., Mistry, M. Optimization-based dynamic motion planning and control for quadruped robots. Nonlinear Dyn 112, 7043–7056 (2024).

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: