Research Papers

Inverse Dynamics for Discrete Geometric Mechanics of Multibody Systems With Application to Direct Optimal Control

[+] Author and Article Information
Staffan Björkenstam

Geometry and Motion Planning
Fraunhofer-Chalmers Centre,
Göteborg SE-412 88, Sweden
e-mail: staffan@fcc.chalmers.se

Sigrid Leyendecker

Chair of Applied Dynamics,
University of Erlangen-Nuremberg,
Erlangen D-91058, Germany
e-mail: sigrid.leyendecker@fau.de

Joachim Linn

Mathematical Methods in
Dynamics and Durability,
Fraunhofer Institute for Industrial Mathematics,
Kaiserslautern D-67663, Germany
e-mail: joachim.linn@itwm.fraunhofer.de

Johan S. Carlson

Geometry and Motion Planning,
Fraunhofer-Chalmers Centre,
Göteborg SE-412 88, Sweden
e-mail: johan.carlson@fcc.chalmers.se

Bengt Lennartson

Signals and Systems,
Chalmers University of Technology,
Göteborg SE-412 96, Sweden
e-mail: bengt.lennartson@chalmers.se

1Corresponding author.

Contributed by the Design Engineering Division of ASME for publication in the JOURNAL OF COMPUTATIONAL AND NONLINEAR DYNAMICS. Manuscript received January 9, 2018; final manuscript received June 28, 2018; published online July 30, 2018. Assoc. Editor: Paramsothy Jayakumar.

J. Comput. Nonlinear Dynam 13(10), 101001 (Jul 30, 2018) (15 pages) Paper No: CND-18-1012; doi: 10.1115/1.4040780 History: Received January 09, 2018; Revised June 28, 2018

In this paper, we present efficient algorithms for computation of the residual of the constrained discrete Euler–Lagrange (DEL) equations of motion for tree structured, rigid multibody systems. In particular, we present new recursive formulas for computing partial derivatives of the kinetic energy. This enables us to solve the inverse dynamics problem of the discrete system with linear computational complexity. The resulting algorithms are easy to implement and can naturally be applied to a very broad class of multibody systems by imposing constraints on the coordinates by means of Lagrange multipliers. A comparison is made with an existing software package, which shows a drastic improvement in computational efficiency. Our interest in inverse dynamics is primarily to apply direct transcription optimal control methods to multibody systems. As an example application, we present a digital human motion planning problem, which we solve using the proposed method. Furthermore, we present detailed descriptions of several common joints, in particular singularity-free models of the spherical joint and the rigid body joint, using the Lie groups of unit quaternions and unit dual quaternions, respectively.

Copyright © 2018 by ASME
Your Session has timed out. Please sign back in to continue.


Marsden, J. E. , and West, M. , 2001, “ Discrete Mechanics and Variational Integrators,” Acta Numer., 10, pp. 357–514. [CrossRef]
Wendlandt, J. M. , and Marsden, J. E. , 1997, “ Mechanical Integrators Derived From a Discrete Variational Principle,” Phys. D: Nonlinear Phenom., 106(3–4), pp. 223–246. [CrossRef]
Leyendecker, S. , Marsden, J. E. , and Ortiz, M. , 2008, “ Variational Integrators for Constrained Dynamical Systems,” ZAMM-J. Appl. Math. Mech., 88(9), pp. 677–708. [CrossRef]
Hairer, E. , Lubich, C. , and Wanner, G. , 2006, Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations, Springer, Berlin.
Leimkuhler, B. , and Reich, S. , 1994, “ Symplectic Integration of Constrained Hamiltonian Systems,” Math. Comput., 63(208), pp. 589–605. [CrossRef]
Leimkuhler, B. , and Patrick, G. W. , 1996, “ A Symplectic Integrator for Riemannian Manifolds,” J. Nonlinear Sci., 6(4), pp. 367–384. [CrossRef]
McLachlan, R. I. , and Quispel, G. R. W. , 2006, “ Geometric Integrators for ODEs,” J. Phys. A: Math. General, 39(19), pp. 5251–5285. [CrossRef]
Reich, S. , 1994, “ Momentum Conserving Symplectic Integrators,” Phys. D: Nonlinear Phenom., 76(4), pp. 375–383. [CrossRef]
Kane, C. , Marsden, J. E. , Ortiz, M. , and West, M. , 2000, “ Variational Integrators and the Newmark Algorithm for Conservative and Dissipative Mechanical Systems,” Int. J. Numer. Methods Eng., 49(10), pp. 1295–1325. [CrossRef]
Lew, A. , Marsden, J. E. , Ortiz, M. , and West, M. , 2004, “ An Overview of Variational Integrators,” Finite Element Methods: 1970's and Beyond, International Center for Numerical Methods in Engineering (CIMNE), Barcelona, Spain, pp. 98–115.
Lew, A. , Marsden, J. E. , Ortiz, M. , and West, M. , 2004, “ Variational Time Integrators,” Int. J. Numer. Methods Eng., 60(1), pp. 153–212. [CrossRef]
Ober-Blöbaum, S. , Junge, O. , and Marsden, J. E. , 2011, “ Discrete Mechanics and Optimal Control: An Analysis,” ESAIM: Control, Optimisation Calculus Variations, 17(2), pp. 322–352. [CrossRef]
Leyendecker, S. , Ober-Blöbaum, S. , Marsden, J. E. , and Ortiz, M. , 2010, “ Discrete Mechanics and Optimal Control for Constrained Systems,” Optim. Control Appl. Methods, 31(6), pp. 505–528. [CrossRef]
Kobilarov, M. , Marsden, J. E. , and Sukhatme, G. S. , 2010, “ Geometric Discretization of Nonholonomic Systems With Symmetries,” Discrete Contin. Dyn. Syst. Ser. S, 3(1), pp. 61–84.
Fetecau, R. C. , Marsden, J. E. , Ortiz, M. , and West, M. , 2003, “ Nonsmooth Lagrangian Mechanics and Variational Collision Integrators,” SIAM J. Appl. Dyn. Syst., 2(3), pp. 381–416. [CrossRef]
Bou-Rabee, N. , and Owhadi, H. , 2008, “ Stochastic Variational Integrators,” IMA J. Numer. Anal., 29(2), pp. 421–443. [CrossRef]
Tao, M. , Owhadi, H. , and Marsden, J. E. , 2010, “ Nonintrusive and Structure Preserving Multiscale Integration of Stiff ODEs, SDEs, and Hamiltonian Systems With Hidden Slow Dynamics Via Flow Averaging,” Multiscale Model. Simul., 8(4), pp. 1269–1324. [CrossRef]
Celledoni, E. , and Owren, B. , 2003, “ Lie Group Methods for Rigid Body Dynamics and Time Integration on Manifolds,” Comput. Methods Appl. Mech. Eng., 192(3–4), pp. 421–438. [CrossRef]
Leok, M. , 2007, “ An Overview of Lie Group Variational Integrators and Their Applications to Optimal Control,” International Conference on Scientific Computation and Differential Equations, Saint-Malo, France, July 9–13, p. 1.
Bou-Rabee, N. , and Marsden, J. E. , 2009, “ Hamilton–Pontryagin Integrators on Lie Groups Part I: Introduction and Structure-Preserving Properties,” Found. Comput. Math., 9(2), pp. 197–219. [CrossRef]
Kobilarov, M. B. , and Marsden, J. E. , 2011, “ Discrete Geometric Optimal Control on Lie Groups,” IEEE Trans. Rob., 27(4), pp. 641–655. [CrossRef]
Arnold, M. , Cardona, A. , and Brüls, O. , 2016, “ A Lie Algebra Approach to Lie Group Time Integration of Constrained Systems,” Structure-Preserving Integrators in Nonlinear Structural Dynamics and Flexible Multibody Dynamics, Springer, Berlin, pp. 91–158. [CrossRef]
Kobilarov, M. , Crane, K. , and Desbrun, M. , 2009, “ Lie Group Integrators for Animation and Control of Vehicles,” ACM Trans. Graph. (TOG), 28(2), pp. 16:1–16:14. [CrossRef]
Lee, T. , Leok, M. , and McClamroch, N. H. , 2017, Global Formulations of Lagrangian and Hamiltonian Dynamics on Manifolds, Springer, Berlin.
Junge, O. , Marsden, J. E. , and Ober-Blöbaum, S. , 2005, “ Discrete Mechanics and Optimal Control,” IFAC Proc. Vol., 38(1), pp. 538–543. [CrossRef]
Betts, J. T. , 2010, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd ed, Society for Industrial and Applied Mathematics, Cambridge University Press, New York. [CrossRef]
Björkenstam, S. , Carlson, J. S. , and Lennartson, B. , 2015, “ Exploiting Sparsity in the Discrete Mechanics and Optimal Control Method With Application to Human Motion Planning,” IEEE International Conference on Automation Science and Engineering (CASE), Gothenburg, Sweden, Aug. 24–28, pp. 769–774.
Featherstone, R. , 1983, “ The Calculation of Robot Dynamics Using Articulated-Body Inertias,” Int. J. Rob. Res., 2(1), pp. 13–30. [CrossRef]
Walker, M. W. , and Orin, D. E. , 1982, “ Efficient Dynamic Computer Simulation of Robotic Mechanisms,” ASME J. Dyn. Syst. Meas. Control, 104(3), pp. 205–211. [CrossRef]
Luh, J. Y. , Walker, M. W. , and Paul, R. P. , 1980, “ On-Line Computational Scheme for Mechanical Manipulators,” ASME J. Dyn. Syst. Meas. Control, 102(2), pp. 69–76. [CrossRef]
Featherstone, R. , 2008, Rigid Body Dynamics Algorithms, Springer, Berlin. [CrossRef]
Jain, A. , 2010, Robot and Multibody Dynamics: Analysis and Algorithms, Springer Science & Business Media, Berlin.
Johnson, E. R. , and Murphey, T. D. , 2009, “ Scalable Variational Integrators for Constrained Mechanical Systems in Generalized Coordinates,” IEEE Trans. Rob., 25(6), pp. 1249–1261. [CrossRef]
Johnson, E. R. , and Murphey, T. D. , 2008, “ Discrete and Continuous Mechanics for Tree Representations of Mechanical Systems,” IEEE International Conference on Robotics and Automation (ICRA), Pasadena, CA, May 19–23, pp. 1106–1111.
Neuroscience and Robotics Lab, 2017, “ Trep: Mechanical Simulation and Optimal Control,” Northwestern University, Evanston, IL, accessed May 23, 2017, http://murpheylab.github.io/trep/
Featherstone, R. , and Orin, D. , 2000, “ Robot Dynamics: Equations and Algorithms,” IEEE International Conference on Robotics and Automation (ICRA), San Francisco, CA, Apr. 24–28, pp. 826–834.
Park, F. C. , Bobrow, J. E. , and Ploen, S. R. , 1995, “ A Lie Group Formulation of Robot Dynamics,” Int. J. Rob. Res., 14(6), pp. 609–618. [CrossRef]
Griewank, A. , and Walther, A. , 2008, Evaluating Derivatives: Principles and Techniques of Algorithmic Differentiation, SIAM, Philadelphia, PA. [CrossRef]
Hall, B. , 2015, Lie Groups, Lie Algebras, and Representations: An Elementary Introduction, Springer, Berlin. [CrossRef]
Murray, R. M. , Li, Z. , Sastry, S. S. , and Sastry, S. S. , 1994, A Mathematical Introduction to Robotic Manipulation, CRC press, Boca Raton, FL.
Selig, J. M. , 2004, Geometric Fundamentals of Robotics, Springer Science & Business Media, Berlin.
Marsden, J. E. , and Ratiu, T. , 2013, Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems, Vol. 17, Springer Science & Business Media, Berlin.
Iserles, A. , Munthe-Kaas, H. Z. , Nørsett, S. P. , and Zanna, A. , 2000, “ Lie-Group Methods,” Acta Numer., 9, pp. 215–365. [CrossRef]
Betts, J. T. , 1998, “ Survey of Numerical Methods for Trajectory Optimization,” J. Guidance, Control, Dynamics, 21(2), pp. 193–207. [CrossRef]
Diehl, M. , Bock, H. G. , Diedam, H. , and Wieber, P.-B. , 2006, “ Fast Direct Multiple Shooting Algorithms for Optimal Robot Control,” Fast Motions in Biomechanics and Robotics, Springer, Berlin, pp. 65–93. In [CrossRef]
Bell, B. M. , 2017, “ CppAD: A Package for C++ Algorithmic Differentiation,” Computational Infrastructure for Operations Research, Reisterstown, MD, accessed July 17, 2018, http://www.coin-or.org/CppAD
Koch, M. W. , Ringkamp, M. , and Leyendecker, S. , 2016, “ Discrete Mechanics and Optimal Control of Walking Gaits,” ASME J. Comput. Nonlinear Dyn., 12(2), p. 021006. [CrossRef]
Falck, A.-C. , Örtengren, R. , and Högberg, D. , 2010, “ The Impact of Poor Assembly Ergonomics on Product Quality: A Cost-Benefit Analysis in Car Manufacturing,” Hum. Factors Ergonom. Manuf. Service Ind., 20(1), pp. 24–41. [CrossRef]
Falck, A.-C. , and Rosenqvist, M. , 2014, “ A Model for Calculation of the Costs of Poor Assembly Ergonomics (Part 1),” Int. J. Ind. Ergonom., 44(1), pp. 140–147. [CrossRef]
Lämkull, D. , Hanson, L. , and Örtengren, R. , 2008, “ Uniformity in Manikin Posturing: A Comparison Between Posture Prediction and Manual Joint Manipulation,” Int. J. Human Factors Modell. Simul., 1(2), pp. 225–243. [CrossRef]
Högberg, D. , Hanson, L. , Bohlin, R. , and Carlson, J. S. , 2016, “ Creating and Shaping the DHM Tool IMMA for Ergonomic Product and Production Design,” Int. J. Digital Hum., 1(2), pp. 132–152. [CrossRef]
Bohlin, R. , Delfs, N. , Hanson, L. , Högberg, D. , and Carlson, J. , 2011, “ Unified Solution of Manikin Physics and Positioning—Exterior Root by Introduction of Extra Parameters,” First International Symposium on Digital Human Modeling (DHM), Lyon, France, June 14–16.
Delfs, N. , Bohlin, R. , Gustafsson, S. , Mårdberg, P. , and Carlson, J. S. , 2014, “ Automatic Creation of Manikin Motions Affected by Cable Forces,” Proc. CIRP, 23, pp. 35–40. [CrossRef]
Pandy, M. G. , Anderson, F. C. , and Hull, D. , 1992, “ A Parameter Optimization Approach for the Optimal Control of Large-Scale Musculoskeletal Systems,” ASME J. Biomech. Eng., 114(4), pp. 450–460. [CrossRef]
Lo, J. , and Metaxas, D. , 1999, “ Recursive Dynamics and Optimal Control Techniques for Human Motion Planning,” IEEE Computer Animation, pp. 220–234.
Xiang, Y. , Chung, H.-J. , Kim, J. H. , Bhatt, R. , Rahmatalla, S. , Yang, J. , Marler, T. , Arora, J. S. , and Abdel-Malek, K. , 2010, “ Predictive Dynamics: An Optimization-Based Novel Approach for Human Motion Simulation,” Struct. Multidiscip. Optim., 41(3), pp. 465–479. [CrossRef]
Maas, R. , and Leyendecker, S. , 2013, “ Biomechanical Optimal Control of Human Arm Motion,” Proc. Inst. Mech. Eng., Part K: J. Multi-Body Dyn., 227(4), pp. 375–389.
Maas, R. , and Leyendecker, S. , 2012, “ Optimal Control of Biomechanical Motion Using Physiologically Motivated Cost Functions,” Second Joint International Conference on Multibody System Dynamics, Stuttgart, Germany, May 29–June 1.
Betts, J. T. , and Frank, P. D. , 1994, “ A Sparse Nonlinear Optimization Algorithm,” J. Optim. Theory Appl., 82(3), pp. 519–541. [CrossRef]
Gerdts, M. , Henrion, R. , Hömberg, D. , and Landry, C. , 2012, “ Path Planning and Collision Avoidance for Robots,” Numer. Algebra, Control Optim., 2(3), pp. 437–463. [CrossRef]
Roller, M. , Björkenstam, S. , Linn, J. , and Leyendecker, S. , 2017, “ Optimal Control of a Biomechanical Multibody Model for the Dynamic Simulation of Working Tasks,” ECCOMAS Thematic Conference on Multibody Dynamics, Prague, Czech Republic, June 19–22, pp. 817–826. http://www.ltd.tf.uni-erlangen.de/Team/Leyendecker/Arxiv/Conference/ECCOMAS2017_MR_SL_biomechanical.pdf
Stewart, D. E. , 2000, “ Rigid-Body Dynamics With Friction and Impact,” SIAM Rev., 42(1), pp. 3–39. [CrossRef]
Harker, P. T. , and Pang, J.-S. , 1990, “ Finite-Dimensional Variational Inequality and Nonlinear Complementarity Problems: A Survey of Theory, Algorithms and Applications,” Math. Program., 48(1–3), pp. 161–220. [CrossRef]
Anitescu, M. , 2005, “ Mathematical Programs With Equilibrium Constraints and Applications to Control,” Argonne National Laboratory, Lemont, IL, Report No. ANL/MCS-P1247-0405. http://www.mcs.anl.gov/papers/P1247.pdf
Posa, M. , Cantu, C. , and Tedrake, R. , 2014, “ A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact,” Int. J. Rob. Res., 33(1), pp. 69–81. [CrossRef]
Diebel, J. , 2006, “ Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors,” Matrix, 58(15–16), pp. 1–35. http://citeseerx.ist.psu.edu/viewdoc/summary?doi=
Wang, X. , Han, D. , Yu, C. , and Zheng, Z. , 2012, “ The Geometric Structure of Unit Dual Quaternion With Application in Kinematic Control,” J. Math. Anal. Appl., 389(2), pp. 1352–1364. [CrossRef]
Baerlocher, P. , and Boulic, R. , 2001, “ Parametrization and Range of Motion of the Ball-and-Socket Joint,” Deformable Avatars, N. Magnenat-Thalmann and D. Thalmann , eds., Vol. 68, Springer, Berlin, pp. 180–190. [CrossRef]


Grahic Jump Location
Fig. 1

Example of a kinematic tree with NB = 7. For link 3, we have κ(3) = {1, 2, 3}, ν(3) = {3, 4, 5, 7}, and μ(3) = {4, 5}.

Grahic Jump Location
Fig. 2

Recursive algorithm for computing the gradients of the kinetic energy, ∇¯qiT and ∇viT, of a kinematic tree of rigid bodies

Grahic Jump Location
Fig. 3

Algorithm for computing the generalized force acting on a kinematic tree from wrenches applied to the rigid bodies

Grahic Jump Location
Fig. 4

Multiple pendulum with NB links

Grahic Jump Location
Fig. 5

Computation times for (a) the constraint function, (b) the Jacobian of the constraints, and (c) the Hessian of the Lagrangian

Grahic Jump Location
Fig. 6

Multibody model of the humanoid. The pelvis is chosen to be the first link in the tree, with index 1, connected to the fixed base, which has index 0. See Table 1 and Appendix A.6 for more details on the bodies and joints of the model.

Grahic Jump Location
Fig. 7

(a) Contact between foot and ground plane, where f∈ℝ3 is the reaction force at the contact point p∈ℝ3 and μ∈ℝ is the coefficient of friction, and (b) octagonal friction cone approximation

Grahic Jump Location
Fig. 8

Snapshots of the solution at (a) t = 0.00 s, (b) t = 0.19 s, (c) t = 0.37 s, (d) t = 0.56 s, (e) t = 0.75 s, and (f) t = 0.94 s

Grahic Jump Location
Fig. 9

(a) Identity configuration of the mechanical system in the optimal control problem in Sec. 5.1.2, i.e., q = IQ and (b) sphere approximation used for collision avoidance in the optimal control problem

Grahic Jump Location
Fig. 10

Reaction forces in the lower back and ankles during the lifting task

Grahic Jump Location
Fig. 11

The transformation, Xρ(i),i, from the body fixed frame of body i, to the body fixed frame of its parent, is a composition of a configuration-dependent transformation XJi(qi), and a constant transformation, XTi, locating joint i in the parent frame, i.e., Xρ(i),i=XTiXJi

Grahic Jump Location
Fig. 12

Recursive algorithm for calculation of the transformations in the tree

Grahic Jump Location
Fig. 13

(a) The universal joint, also known as Cardan or Hooke's joint, gives rotational motion about two orthogonal axes. (b) The configuration of the ball and socket joint belongs to SO(3). Motion in SO(3) can be decomposed into swing and twist motion.



Some tools below are only available to our subscribers or users with an online account.

Related Content

Customize your page view by dragging and repositioning the boxes below.

Related Journal Articles
Related eBook Content
Topic Collections

Sorry! You do not have access to this content. For assistance or to subscribe, please contact us:

  • TELEPHONE: 1-800-843-2763 (Toll-free in the USA)
  • EMAIL: asmedigitalcollection@asme.org
Sign In