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.

References

References
1.
Marsden
,
J. E.
, and
West
,
M.
,
2001
, “
Discrete Mechanics and Variational Integrators
,”
Acta Numer.
,
10
, pp.
357
514
.
2.
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
.
3.
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
.
4.
Hairer
,
E.
,
Lubich
,
C.
, and
Wanner
,
G.
,
2006
,
Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations
,
Springer
, Berlin.
5.
Leimkuhler
,
B.
, and
Reich
,
S.
,
1994
, “
Symplectic Integration of Constrained Hamiltonian Systems
,”
Math. Comput.
,
63
(
208
), pp.
589
605
.
6.
Leimkuhler
,
B.
, and
Patrick
,
G. W.
,
1996
, “
A Symplectic Integrator for Riemannian Manifolds
,”
J. Nonlinear Sci.
,
6
(
4
), pp.
367
384
.
7.
McLachlan
,
R. I.
, and
Quispel
,
G. R. W.
,
2006
, “
Geometric Integrators for ODEs
,”
J. Phys. A: Math. General
,
39
(
19
), pp.
5251
5285
.
8.
Reich
,
S.
,
1994
, “
Momentum Conserving Symplectic Integrators
,”
Phys. D: Nonlinear Phenom.
,
76
(
4
), pp.
375
383
.
9.
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
.
10.
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.
11.
Lew
,
A.
,
Marsden
,
J. E.
,
Ortiz
,
M.
, and
West
,
M.
,
2004
, “
Variational Time Integrators
,”
Int. J. Numer. Methods Eng.
,
60
(
1
), pp.
153
212
.
12.
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
.
13.
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
.
14.
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
.
15.
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
.
16.
Bou-Rabee
,
N.
, and
Owhadi
,
H.
,
2008
, “
Stochastic Variational Integrators
,”
IMA J. Numer. Anal.
,
29
(
2
), pp.
421
443
.
17.
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
.
18.
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
.
19.
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.
20.
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
.
21.
Kobilarov
,
M. B.
, and
Marsden
,
J. E.
,
2011
, “
Discrete Geometric Optimal Control on Lie Groups
,”
IEEE Trans. Rob.
,
27
(
4
), pp.
641
655
.
22.
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
.
23.
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
.
24.
Lee
,
T.
,
Leok
,
M.
, and
McClamroch
,
N. H.
,
2017
,
Global Formulations of Lagrangian and Hamiltonian Dynamics on Manifolds
,
Springer
, Berlin.
25.
Junge
,
O.
,
Marsden
,
J. E.
, and
Ober-Blöbaum
,
S.
,
2005
, “
Discrete Mechanics and Optimal Control
,”
IFAC Proc. Vol.
,
38
(
1
), pp.
538
543
.
26.
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.
27.
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
.
28.
Featherstone
,
R.
,
1983
, “
The Calculation of Robot Dynamics Using Articulated-Body Inertias
,”
Int. J. Rob. Res.
,
2
(
1
), pp.
13
30
.
29.
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
.
30.
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
.
31.
Featherstone
,
R.
,
2008
,
Rigid Body Dynamics Algorithms
,
Springer
, Berlin.
32.
Jain
,
A.
,
2010
,
Robot and Multibody Dynamics: Analysis and Algorithms
,
Springer Science & Business Media
, Berlin.
33.
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
.
34.
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
.
35.
Neuroscience and Robotics Lab, 2017, “
Trep: Mechanical Simulation and Optimal Control
,” Northwestern University, Evanston, IL, accessed May 23, 2017, http://murpheylab.github.io/trep/
36.
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
.
37.
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
.
38.
Griewank
,
A.
, and
Walther
,
A.
,
2008
,
Evaluating Derivatives: Principles and Techniques of Algorithmic Differentiation
,
SIAM
, Philadelphia, PA.
39.
Hall
,
B.
,
2015
,
Lie Groups, Lie Algebras, and Representations: An Elementary Introduction
,
Springer
, Berlin.
40.
Murray
,
R. M.
,
Li
,
Z.
,
Sastry
,
S. S.
, and
Sastry
,
S. S.
,
1994
,
A Mathematical Introduction to Robotic Manipulation
,
CRC press
, Boca Raton, FL.
41.
Selig
,
J. M.
,
2004
,
Geometric Fundamentals of Robotics
,
Springer Science & Business Media
, Berlin.
42.
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.
43.
Iserles
,
A.
,
Munthe-Kaas
,
H. Z.
,
Nørsett
,
S. P.
, and
Zanna
,
A.
,
2000
, “
Lie-Group Methods
,”
Acta Numer.
,
9
, pp.
215
365
.
44.
Betts
,
J. T.
,
1998
, “
Survey of Numerical Methods for Trajectory Optimization
,”
J. Guidance, Control, Dynamics
,
21
(
2
), pp.
193
207
.
45.
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
46.
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
47.
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
.
48.
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
.
49.
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
.
50.
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
.
51.
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
.
52.
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.
53.
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
.
54.
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
.
55.
Lo
,
J.
, and
Metaxas
,
D.
,
1999
, “
Recursive Dynamics and Optimal Control Techniques for Human Motion Planning
,” IEEE
Computer Animation,
pp.
220
234
.
56.
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
.
57.
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
.
58.
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.
59.
Betts
,
J. T.
, and
Frank
,
P. D.
,
1994
, “
A Sparse Nonlinear Optimization Algorithm
,”
J. Optim. Theory Appl.
,
82
(
3
), pp.
519
541
.
60.
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
.
61.
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
62.
Stewart
,
D. E.
,
2000
, “
Rigid-Body Dynamics With Friction and Impact
,”
SIAM Rev.
,
42
(
1
), pp.
3
39
.
63.
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
.
64.
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
65.
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
.
66.
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=10.1.1.110.5134
67.
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
.
68.
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
.
This content is only available via PDF.
You do not currently have access to this content.