On the integration of singularity-free representations of \(\varvec{SO(3)}\) for direct optimal control

  • 229 Accesses

  • 2 Citations


In this paper we analyze the performance of different combinations of: (1) parameterization of the rotational degrees of freedom (DOF) of multibody systems, and (2) choice of the integration scheme, in the context of direct optimal control discretized according to the direct multiple-shooting method. The considered representations include quaternions and Direction Cosine Matrices, both having the peculiarity of being non-singular and requiring more than three parameters to describe an element of the Special Orthogonal group \(\textit{SO}(3)\). These representations yield invariants in the dynamics of the system, i.e., algebraic conditions which have to be satisfied in order for the model to be representative of physical reality. The investigated integration schemes include the classical explicit Runge–Kutta method, its stabilized version based on Baumgarte’s technique, which tends to reduce the drift from the underlying manifold, and a structure-preserving alternative, namely the Runge–Kutta Munthe-Kaas method, which preserves the invariants by construction. The performances of the combined choice of representation and integrator are assessed by solving thousands of planning tasks for a nonholonomic, underactuated cart-pendulum system, where the pendulum can experience arbitrarily large 3D rotations. The aspects analyzed include success rate, average number of iterations and CPU time to convergence, and quality of the solution. The results reveal how structure-preserving integrators are the only choice for lower accuracies, whereas higher-order, non-stabilized standard integrators seem to be the computationally most competitive solution when higher levels of accuracy are pursued. Overall, the quaternion-based representation is the most efficient in terms of both iterations and CPU time to convergence, albeit at the cost of lower success rates and increased probability of being trapped by higher local minima.

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

Access options

Buy single article

Instant unlimited access to the full article PDF.

US$ 39.95

Price includes VAT for USA

Subscribe to journal

Immediate online access to all issues from 2019. Subscription will auto renew annually.

US$ 199

This is the net price. Taxes to be calculated in checkout.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12


  1. 1.

    Hairer, E., Lubich, C., Wanner, G.: Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations, vol. 31. Springer, Berlin (2006)

  2. 2.

    Brüls, O., Cardona, A.: On the use of Lie group time integrators in multibody dynamics. J. Comput. Nonlinear Dyn. 5(3), 031,002 (2010)

  3. 3.

    Betsch, P.: Structure-Preserving Integrators in Nonlinear Structural Dynamics and Flexible Multibody Dynamics. Courses and Lectures. Springer, Berlin (2016)

  4. 4.

    Stuelpnagel, J.: On the parametrization of the three-dimensional rotation group. SIAM Rev. 6(4), 422–430 (1964)

  5. 5.

    Shuster, M.D.: A survey of attitude representations. Navigation 8(9), 439–517 (1993)

  6. 6.

    Marsden, J.E., Ratiu, T.: Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems, vol. 17. Springer, Berlin (2013)

  7. 7.

    Gros, S., Zanon, M., Diehl, M.: Baumgarte stabilisation over the SO (3) rotation group for control. In: 2015 54th IEEE Conference on Decision and Control (CDC), pp. 620–625. IEEE (2015)

  8. 8.

    Baumgarte, J.: Stabilization of constraints and integrals of motion in dynamical systems. Comput. Methods Appl. Mech. Eng. 1(1), 1–16 (1972)

  9. 9.

    Iserles, A., Munthe-Kaas, H.Z., Nørsett, S.P., Zanna, A.: Lie-group methods. Acta Numer. 2000(9), 215–365 (2000)

  10. 10.

    Celledoni, E., Owren, B.: Lie group methods for rigid body dynamics and time integration on manifolds. Comput. Methods Appl. Mech. Eng. 192(3), 421–438 (2003)

  11. 11.

    Engø, K., Marthinsen, A.: Modeling and solution of some mechanical problems on Lie groups. Multibody Syst. Dyn. 2(1), 71–88 (1998)

  12. 12.

    Park, J., Chung, W.K.: Geometric integration on Euclidean group with application to articulated multibody systems. IEEE Trans. Robot. 21(5), 850–863 (2005)

  13. 13.

    Bottasso, C.L., Borri, M.: Integrating finite rotations. Comput. Methods Appl. Mech. Eng. 164(3), 307–331 (1998)

  14. 14.

    Crouch, P.E., Grossman, R.: Numerical integration of ordinary differential equations on manifolds. J. Nonlinear Sci. 3(1), 1–33 (1993)

  15. 15.

    Munthe-Kaas, H.: Runge–Kutta methods on Lie groups. BIT Numer. Math. 38(1), 92–111 (1998)

  16. 16.

    Terze, Z., Müller, A., Zlatar, D.: Lie-group integration method for constrained multibody systems in state space. Multibody Syst. Dyn. 34(3), 275–305 (2015)

  17. 17.

    Terze, Z., Müller, A., Zlatar, D.: Singularity-free time integration of rotational quaternions using non-redundant ordinary differential equations. Multibody Syst. Dyn. 38(3), 201–225 (2015)

  18. 18.

    Betts, J.T.: Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd edn. SIAM, Philadelphia (2010)

  19. 19.

    Bottasso, C.L., Croce, A.: Optimal control of multibody systems using an energy preserving direct transcription method. Multibody syst. Dyn. 12(1), 17–45 (2004)

  20. 20.

    Chung, C.Y., Lee, J.W., Lee, S.M., Lee, B.H.: Balancing of an inverted pendulum with a redundant direct-drive robot. In: Proceedings ICRA’00 IEEE International Conference on Robotics and Automation 2000, vol. 4, pp. 3952–3957. IEEE (2000)

  21. 21.

    Yang, R., Kuen, Y.Y., Li, Z.: Stabilization of a 2-DOF spherical pendulum on xy table. In: Proceedings of the 2000 IEEE International Conference on Control Applications 2000, pp. 724–729. IEEE (2000)

  22. 22.

    Albouy, X., Praly, L.: On the use of dynamic invariants and forwarding for swinging up a spherical inverted pendulum. In: Proceedings of the 39th IEEE Conference on Decision and Control 2000, vol. 2, pp. 1667–1672. IEEE (2000)

  23. 23.

    Liu, G., Nešić, D., Mareels, I.: Modelling and stabilisation of a spherical inverted pendulum. IFAC Proc. Vol. 38(1), 1130–1135 (2005)

  24. 24.

    Do, K.D., Seet, G.: Motion control of a two-wheeled mobile vehicle with an inverted pendulum. J. Intel. Robot. Syst. 60(3), 577–605 (2010)

  25. 25.

    Jung, S., Kim, S.S.: Control experiment of a wheel-driven mobile inverted pendulum using neural network. IEEE Trans. Control Syst. Technol. 16(2), 297–303 (2008)

  26. 26.

    Yue, M., Wei, X., Li, Z.: Adaptive sliding-mode control for two-wheeled inverted pendulum vehicle based on zero-dynamics theory. Nonlinear Dyn. 76(1), 459–471 (2014)

  27. 27.

    Yoon, M.G.: Dynamics and stabilization of a spherical inverted pendulum on a wheeled cart. Int. J. Control Autom. Syst. 8(6), 1271–1279 (2010)

  28. 28.

    Shabana, A.A.: Computational Dynamics. Wiley, New York (2009)

  29. 29.

    Shabana, A.A.: Dynamics of Multibody Systems, 4th edn. Cambridge University Press, Cambridge (2013)

  30. 30.

    Holm, D.D.: Geometric Mechanics, Part II: Rotating, Translating and Rolling. Imperial College Press, London (2011)

  31. 31.

    Nocedal, J., Wright, S.: Numerical Optimization. Springer, New York (2006)

  32. 32.

    Sternberg, J., Gros, S., Houska, B., Diehl, M.: Approximate robust optimal control of periodic systems with invariants and high-index differential algebraic systems. IFAC Proc. Vol. 45(13), 690–695 (2012)

  33. 33.

    Betts, J.T.: Survey of numerical methods for trajectory optimization. J. Guid. Control Dyn. 21(2), 193–207 (1998)

  34. 34.

    Bock, H.G., Plitt, K.J.: A multiple shooting algorithm for direct solution of optimal control problems. In: Proceedings of the IFAC World Congress (1984)

  35. 35.

    Bryson, A.E., Ho, Y.C.: Applied Optimal Control. Wiley, New York (1975)

  36. 36.

    Hager, W.W.: Runge–Kutta methods in optimal control and the transformed adjoint system. Numer. Math. 87(2), 247–282 (2000)

  37. 37.

    Gong, Q., Ross, I.M., Kang, W., Fahroo, F.: On the pseudospectral covector mapping theorem for nonlinear optimal control. In: 2006 45th IEEE Conference on Decision and Control, pp. 2679–2686. IEEE (2006)

  38. 38.

    Hairer, E., Nørsett, S., Wanner, G.: Solving ordinary differential equations I: nonstiff problems. Springer Series in Computational Mathematics, vol. 8. Springer, Berlin (1993)

  39. 39.

    Ascher, U.M.: Stabilization of invariants of discretized differential systems. Numer. Algorithms 14(1–3), 1–24 (1997)

  40. 40.

    Erhard, M., Horn, G., Diehl, M.: A quaternion-based model for optimal control of the SkySails airborne wind energy system. Zeitschrift für Angewandte Mathematik und Mechanik 97(1), 7–24 (2017). doi:10.1002/zamm.201500180

  41. 41.

    Gros, S., Zanon, M., Vukov, M., Diehl, M.: Nonlinear MPC and MHE for mechanical multi-body systems with application to fast tethered airplanes. IFAC Proc. Vol. 45(17), 86–93 (2012)

  42. 42.

    Andrle, M.S., Crassidis, J.L.: Geometric integration of quaternions. J. Guid. Control Dyn. 36(6), 1762–1767 (2013)

  43. 43.

    Murray, R.M., Li, Z.: A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton (1994)

  44. 44.

    Andersson, J.: A general-purpose software framework for dynamic optimization. PhD thesis, Arenberg Doctoral School, Katholieke Universiteit Leuven (2013)

  45. 45.

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

  46. 46.

    Griewank, A., Walther, A.: Evaluating Derivatives: Principles and Techniques of Algorithmic Differentiation. SIAM, Philadelphia (2008)

  47. 47.

    Lago Garcia, J.: Periodic optimal control and model predictive control of a tethered kite for airborne wind energy. Master’s thesis, University of Freiburg (2016)

  48. 48.

    Manara, S., Gabiccini, M., Artoni, A., Diehl, M.: On the integration of singularity-free representations of SO(3) for direct optimal control—submission accompanying video. (2017)

Download references


This work was partially supported by Grant No. 645599 ”SoMa” (Soft-bodied intelligence for Manipulation) within the H2020-ICT-2014-1 program. Support by the EU via ERC-HIGHWIND (259 166), ITN-TEMPO (607957) and ITN-AWESCO (642 682), by DFG via Research Unit FOR 2401, and by the German BMWi via the project eco4wind is also gratefully acknowledged. Marco Gabiccini wishes to thank the colleagues Luca Greco and Paolo Mason from the Laboratoire des Signaux & Systèmes, CentraleSupélec for having initially disclosed the benchmark mechanical system investigated in this paper.

Author information

Correspondence to Silvia Manara.

Electronic supplementary material

Below is the link to the electronic supplementary material.

Supplementary material 1 (mp4 22363 KB)

Supplementary material 1 (mp4 22363 KB)

Rights and permissions

Reprints and Permissions

About this article

Verify currency and authenticity via CrossMark

Cite this article

Manara, S., Gabiccini, M., Artoni, A. et al. On the integration of singularity-free representations of \(\varvec{SO(3)}\) for direct optimal control. Nonlinear Dyn 90, 1223–1241 (2017).

Download citation


  • Special orthogonal group \(\textit{SO}(3)\)
  • Rotation parameterization
  • Direction Cosine Matrix
  • Quaternions
  • Lie group integrators
  • Direct optimal control
  • Spherical pendulum
  • Unicycle