DMOC–Based Robot Trajectory Optimization with Analytical First–Order Information

Villanueva-Piñon, Carla; Arechavaleta, Gustavo

doi:10.61822/amcs-2025-0007

Abstract

Discrete mechanics and optimal control (DMOC) is a numerical optimal control framework capable of solving robot trajectory optimization problems. This framework has advantages over other direct collocation and multiple-shooting schemes. In particular, it works with a reduced number of decision variables due to the use of the forced discrete Euler–Lagrange (DEL) equation. Also, the transcription mechanism inherits the numerical benefits of variational integrators (i.e., momentum and energy conservation over a long time horizon with large time steps). We extend the benefits of DMOC to solve trajectory optimization problems for highly articulated robotic systems. We provide analytical evaluations of the forced DEL equation and its partial differentiation with respect to decision variables. The Lie group formulation of rigid-body motion and the use of multilinear algebra allow us to efficiently handle sparse tensor computations. The arithmetic complexity of the proposed strategy is analyzed, and it is validated by solving humanoid motion problems.

References

Agamawi, Y.M. and Rao, A.V. (2020). CGPOPS: A C++ software for solving multiple-phase optimal control problems using adaptive Gaussian quadrature collocation and sparse nonlinear programming, ACM Transactions on Mathematical Software 46(3): 1–38.
Search in Google Scholar Back to article
Aguilar-Ibanez, C., Suarez-Castanon, M.S., Saldivar, B., Jimenez-Lizarraga, M.A., de Jesus Rubio, J. and Mendoza-Mendoza, J. (2024). Algebraic active disturbance rejection to control a generalized uncertain second-order flat system, International Journal of Applied Mathematics and Computer Science 34(2): 185–198, DOI: 10.61822/amcs-2024-0013.
Search in Google Scholar Back to article
Andersson, J.A., Gillis, J., Horn, G., Rawlings, J.B. and Diehl, M. (2019). CasADi: A software framework for nonlinear optimization and optimal control, Mathematical Programming Computation 11(1): 1–36.
Search in Google Scholar Back to article
Becerra, V.M. (2010). Solving complex optimal control problems at no cost with PSOPT, 2010 IEEE International Symposium on Computer-Aided Control System Design, Yokohama, Japan, pp. 1391–1396.
Search in Google Scholar Back to article
Betts, J.T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, SIAM, Philadelphia.
Search in Google Scholar Back to article
Betts, J.T. and Erb, S.O. (2003). Optimal low thrust trajectories to the moon, SIAM Journal on Applied Dynamical Systems 2(2): 144–170.
Search in Google Scholar Back to article
Biegler, L.T. and Zavala, V.M. (2009). Large-scale nonlinear programming using IPOPT: An integrating framework for enterprise-wide dynamic optimization, Computers & Chemical Engineering 33(3): 575–582.
Search in Google Scholar Back to article
Brockett, R.W. (2005). Robotic manipulators and the product of exponentials formula, in P.A. Fuhrmanni (Ed.), Mathematical Theory of Networks and Systems, Springer, Berlin, pp. 120–129.
Search in Google Scholar Back to article
Budhiraja, R., Carpentier, J., Mastalli, C. and Mansard, N. (2018). Differential dynamic programming for multi-phase rigid contact dynamics, 2018 IEEE–RAS 18th International Conference on Humanoid Robots (Humanoids), Beijing, China, pp. 1–9.
Search in Google Scholar Back to article
Cardona-Ortiz, D., Paz, A. and Arechavaleta, G. (2020). Exploiting sparsity in robot trajectory optimization with direct collocation and geometric algorithms, 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, pp. 469–475.
Search in Google Scholar Back to article
Carlier, G. (2022). Classical and Modern Optimization, World Scientific, London.
Search in Google Scholar Back to article
Fan, T., Schultz, J. and Murphey, T. (2018). Efficient computation of higher-order variational integrators in robotic simulation and trajectory optimization, in M. Morales et al. (Eds), Algorithmic Foundations of Robotics, Springer, Cham, pp. 689–706.
Search in Google Scholar Back to article
Featherstone, R. (2014). Rigid Body Dynamics Algorithms, Springer, New York.
Search in Google Scholar Back to article
Hereid, A. and Ames, A.D. (2017). FROST^∗: Fast robot optimization and simulation toolkit, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, Canada, pp. 719–726.
Search in Google Scholar Back to article
Houska, B., Ferreau, H.J. and Diehl, M. (2011). ACADO toolkit—An open-source framework for automatic control and dynamic optimization, Optimal Control Applications and Methods 32(3): 298–312.
Search in Google Scholar Back to article
Howell, T.A., Jackson, B.E. and Manchester, Z. (2019). ALTRO: A fast solver for constrained trajectory optimization, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, pp. 7674–7679.
Search in Google Scholar Back to article
Johnson, E.R. and Murphey, T.D. (2009). Scalable variational integrators for constrained mechanical systems in generalized coordinates, IEEE Transactions on Robotics 25(6): 1249–1261.
Search in Google Scholar Back to article
Johnson, E., Schultz, J. and Murphey, T. (2015). Structured linearization of discrete mechanical systems for analysis and optimal control, IEEE Transactions on Automation Science and Engineering 12(1): 140–152.
Search in Google Scholar Back to article
Kelly, M. (2017). An introduction to trajectory optimization: How to do your own direct collocation, SIAM Review 59(4): 849–904.
Search in Google Scholar Back to article
Kelly, M.P. (2019). DirCol5i: Trajectory optimization for problems with high-order derivatives, Journal of Dynamic Systems, Measurement, and Control 141(3).
Search in Google Scholar Back to article
Kobilarov, M.B. and Marsden, J.E. (2011). Discrete geometric optimal control on Lie groups, IEEE Transactions on Robotics 27(4): 641–655.
Search in Google Scholar Back to article
Kobilarov, M. and Sukhatme, G. (2007). Optimal control using nonholonomic integrators, Proceedings 2007 IEEE International Conference on Robotics and Automation, Rome, Italy, pp. 1832–1837.
Search in Google Scholar Back to article
Lee, J., Liu, C.K., Park, F.C. and Srinivasa, S.S. (2020). A linear-time variational integrator for multibody systems, in K. Goldberg et al. (Eds), Algorithmic Foundations of Robotics XII, Springer, Cham, pp. 352–367.
Search in Google Scholar Back to article
Leineweber, D. B., Sch¨afer, A., Bock, H.G. and Schlöder, J. P. (2003). An efficient multiple shooting based reduced SQP strategy for large-scale dynamic process optimization. Part II: Software aspects and applications, Computers & Chemical Engineering 27(2): 167–174.
Search in Google Scholar Back to article
Leyendecker, S., Ober-Blobaum, S., Marsden, J.E. and Ortiz, M. (2010). Discrete mechanics and optimal control for constrained systems, Optimal Control Applications and Methods 31(6): 505–528.
Search in Google Scholar Back to article
Liu, G., Wu, S., Zhu, L., Wang, J. and Lv, Q. (2022). Fast and smooth trajectory planning for a class of linear systems based on parameter and constraint reduction, International Journal of Applied Mathematics and Computer Science 32(1): 11–21, DOI: 10.34768/amcs-2022-0002.
Search in Google Scholar Back to article
Manchester, Z., Doshi, N., Wood, R.J. and Kuindersma, S. (2019). Contact-implicit trajectory optimization using variational integrators, International Journal of Robotics Research 38(12-13): 1463–1476.
Search in Google Scholar Back to article
Manns, P. and Mombaur, K. (2017). Towards discrete mechanics and optimal control for complex models, IFACPapersOnLine 50(1): 4812–4818.
Search in Google Scholar Back to article
Marsden, J.E. and West, M. (2001). Discrete mechanics and variational integrators, Acta Numerica 10(10): 357–514.
Search in Google Scholar Back to article
Mastalli, C., Budhiraja, R., Merkt, W., Saurel, G., Hammoud, B., Naveau, M., Carpentier, J., Righetti, L., Vijayakumar, S. and Mansard, N. (2020). Crocoddyl: An efficient and versatile framework for multi-contact optimal control, 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, pp. 2536–2542.
Search in Google Scholar Back to article
Mayne, D. (1966). A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems, International Journal of Control 3(1): 85–95.
Search in Google Scholar Back to article
Ober-Blöbaum, S., Junge, O. and Marsden, J.E. (2011). Discrete mechanics and optimal control: An analysis, ESAIM: Control, Optimisation and Calculus of Variations 17(2): 322–352.
Search in Google Scholar Back to article
Orin, D.E., Goswami, A. and Lee, S.-H. (2013). Centroidal dynamics of a humanoid robot, Autonomous Robots 35(2-3): 161–176.
Search in Google Scholar Back to article
Park, F.C., Kim, B., Jang, C. and Hong, J. (2018). Geometric algorithms for robot dynamics: A tutorial review, Applied Mechanics Reviews 70(1): 010803.
Search in Google Scholar Back to article
Rao, A.V. (2009). A survey of numerical methods for optimal control, Advances in the Astronautical Sciences 135(1): 497–528.
Search in Google Scholar Back to article
Rohmer, E., Singh, S.P.N. and Freese, M. (2013). V-REP: A versatile and scalable robot simulation framework, International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, pp. 1321–1326.
Search in Google Scholar Back to article
Selig, J.M. (2004). Geometric Fundamentals of Robotics, Springer, New York.
Search in Google Scholar Back to article
Sun, Z., Tian, Y., Li, H. and Wang, J. (2016). A superlinear convergence feasible sequential quadratic programming algorithm for bipedal dynamic walking robot via discrete mechanics and optimal control, Optimal Control Applications and Methods 37(6): 1139–1161.
Search in Google Scholar Back to article
Zhang, W., Wang, D. and Inanc, T. (2018). A multiphase DMOC-based trajectory optimization method, Optimal Control Applications and Methods 39(1): 114–129.
Search in Google Scholar Back to article

DMOC–Based Robot Trajectory Optimization with Analytical First–Order Information

Abstract

Paradigm

My account