CAKDVolIISecondEd1 10 2021

COMPUTER-AIDED KINEMATICS AND DYNAMICS
OF MECHANICAL SYSTEMS
Volume II: Modern Methods
Edward J. Haug
Carver Distinguished Professor Emeritus
Department of Mechanical and Industrial Engineering
The University of Iowa
echaug@gmail.com
Second Edition
January 2021
Copyright 2021
All rights reserved
Cite as follows:
Haug, E.J., 2021, Computer-Aided Kinematics and Dynamics of Mechanical Systems, Volume
II: Modern Methods, Second Edition, ResearchGate, www.researchgate.net.
i
PREFACE
This electronic text uses modern methods of differential geometry to (1) obtain ordinary
differential equations (ODE) of motion for mechanical system dynamics, (2) construct numerical
algorithms for their solution, and (3) implement equation construction and numerical solution in
the form of MATLAB computer code. A first volume of this series (Haug, 1989), Computer-
Aided Kinematics and Dynamics of Mechanical Systems Volume I: Basic Methods, was based
on the Lagrange multiplier form of differential-algebraic equations (DAE) of motion that was
state-of-the-art in the late 1900s.
DAE vs ODE of Dynamics
While the DAE formulation of mechanical system dynamics is easily derived, the
resulting equations are extraordinarily difficult to solve. A landmark paper by Petzold (1982),
entitled roughly “DAE are not ODE”, identified severe challenges in solving the Index 3 DAE of
multibody dynamics. Nevertheless, the literature has continued for half a century to focus on
methods for numerical solution of DAE. In stark contrast with the DAE approach, ODE
formulations are more difficult to derive, but once obtained can be reliably and accurately solved
using well established numerical integration methods.
A manifold theoretic foundation for ODE formulation of mechanical system dynamics is
presented, based on the mathematical theory of differential geometry. A fascinating insight
provided by this approach is that Lagrange multipliers and the associated DAE of dynamics
could have been avoided for two and one half centuries since landmark dynamics contributions
by d’Alembert (1743) and Lagrange (1788) in the 18th century. Lagrange multipliers are
introduced in Chapter 4 of the text only to represent constraint reaction forces and are not used in
formulating ODE of mechanical system dynamics for holonomic systems in Chapter 5,
nonholonomic systems in Chapter 6, and manipulators in Chapter 9. It is interesting to ruminate
on what might have been a world without Lagrange multipliers and DAE. In hindsight, one
might conclude that “the only good DAE is an ODE”.
The Modern Focus of Computational Kinematics and Dynamics
This text presents foundations for computational kinematics and dynamics of planar and
spatial mechanical systems. The approach taken emphasizes systematic methods for formulation
and numerical solution of the governing equations of kinematics and dynamics. Equally
important is the creation of numerical algorithms and their implementation in MATLAB
computer code that reliably simulates the kinematics and dynamics of a broad spectrum of
mechanical systems.
The focus of the text is on simulation of mechanical systems that are encountered in
diverse fields of application. Emphasis is placed on modeling mechanical systems and
implementing equations of kinematics and dynamics in MATLAB computer code that is made
available with the text for use in analyzing the motion of examples presented and creating
simulations of mechanical systems of interest to the reader. While emphasis is on spatial
systems, less complicated planar systems that make concepts easier to grasp are studied. In
addition to code for specific examples, moderately general-purpose codes are provided for
simulation of planar and spatial mechanical system kinematics and dynamics.
ii
The goal is an electronic text that is based on a minimal set of fundamental laws of
mechanics, basic mathematics, and numerical methods that provide the foundation for
computational treatment of mechanical system kinematics and dynamics. Rather than publishing
yet another printed book on the subject, the intention is to create an electronic document that can
be used and evolved by faculty teaching courses that introduce students to the field and by
practicing engineers. Computer implementation and demonstration of methods are presented, in
the form of MATLAB programs that can be used by students and practicing engineers in the
learning process and in applications.
Organization of the Text
Basic elements of mechanical system dynamics are introduced in Chapter 1, using
examples to identify challenges and opportunities to be encountered. The nonlinear character of
mechanical system performance is emphasized, to set the stage for developments that follow.
The body of the text begins with Chapter 2 on representation of position and orientation
of bodies, with primary focus on spatial motion. Basics of vector analysis, matrix algebra,
multivariable calculus, and differential geometry are summarized, as the basis for a rigorous but
practical formulation of mechanical system kinematics and dynamics.
Libraries of kinematic joints between pairs of bodies are presented in Chapter 3,
including associated velocity and acceleration equations. Methods for kinematic analysis on a
time grid are presented and singularities that can arise in improperly designed or poorly modeled
mechanisms are analyzed, with the help of examples. Rudimentary general-purpose MATLAB
computer codes are provided for kinematic analysis of planar and spatial systems. Results using
the codes for a number of examples are presented
A variational formulation of the equations of motion for rigid bodies and multibody
systems is presented in Chapter 4. Equations of motion are derived for particles, planar rigid
bodies, and spatial rigid bodies, based on only Newton's three laws of dynamics of particles and
d’Alembert’s variational principle for constrained systems of rigid bodies. Runge-Kutta
numerical integration methods for both first and second order ODE are presented and
implemented, in the form of MATLAB computer code that is provided. Numerical solutions of
ODE for examples treated in the chapter are presented and analyzed. Constraint reaction forces
associated with joints between bodies are represented using Lagrange multipliers and computed
using results of dynamic simulation.
A broadly applicable ODE formulation of multibody dynamics for holonomically
constrained systems is presented in Chapter 5, using a constraint tangent space differential
geometry formulation. Conditions for existence, uniqueness, and differentiability of solutions of
the resulting equations of motion with respect to design parameters are presented. Solution
algorithms are presented that enable application of explicit and implicit ODE numerical
integration methods with error control. Examples are presented to test methods and provide the
basis for experimentation and analysis. Index 0 DAE that are algebraically equivalent to ODE
are derived, using the tangent space formulation, and implemented with both explicit and
implicit numerical integration algorithms. Rudimentary general-purpose MATLAB computer
codes are provided for simulation of planar and spatial multibody system dynamics. They are
used in applications of moderate complexity that would be oppressive if treated with ad-hoc
derivation and programming of the highly nonlinear equations of motion.
iii
While nonholonomic systems are not common in mechanical system dynamics,
significant applications motivate their treatment in Chapter 6. The tangent space ODE
formulation of Chapter 5 is extended to provide a broadly applicable formulation and numerical
solution methods. MATLAB computer codes for treatment of examples and studies of
excursions are provided.
A basic theory of the DAE of holonomic multibody dynamics is presented in Chapter 7,
based on existence and uniqueness of solutions and continuous dependence of solutions on
model parameters presented in Chapter 5. Methods for obtaining approximate solutions of the
DAE are presented and implemented in rudimentary general-purpose MATLAB computer codes
for simulation of planar and spatial systems. Examples are studied to compare properties of DAE
solution methods with those of ODE methods presented in Chapter 5.
Friction effects in multibody systems are introduced in Chapter 8. Kinematic
formulations are presented that account for contact geometry in joints and determine constraint
contact forces, as functions of Lagrange multipliers in the Index 0 DAE of dynamics. Continuous
models of friction forces are introduced as realistic models of multibody dynamics with friction,
without complexities associated with the physically dubious discontinuous Coulomb friction
model. Situations that lead to static friction lock, or stiction, that occur in applications are
studied. Numerical solution methods based on the Index 0 DAE formulation and implicit
numerical solvers are presented and shown to yield realistic solutions of problems that include
friction and stiction effects in planar and spatial mechanical systems. Rudimentary general-
purpose MATLAB computer codes are provided, for dynamic simulation of planar and spatial
mechanical systems with friction.
A manipulator kinematics formulation that includes input coordinates, mechanism
generalized coordinates, and output coordinates is presented in Chapter 9. Criteria that assure
local existence and uniqueness of forward kinematic mappings that are needed in manipulator
control and inverse kinematic mappings that are needed in manipulator programming are
presented. It is shown that these mappings define a differentiable manifold structure for the
manipulator configuration space. Differential geometry results of Chapter 3 show that the regular
manipulator configuration space is composed of maximal, disjoint, path connected, singularity
free domains of functionality on which the manipulator can be reliably programmed and
controlled. These domains and extensions that uniformly avoid singularities are constructed and
mapped for planar and spatial examples. Finally, ODE of manipulator dynamics are embedded in
singularity free domains of functionality, without the need for ad-hoc derivation.
This organization has been selected to permit the student and practicing engineer to
master basic concepts and to gain experience in modeling and analysis, prior to building upon the
foundation for large scale dynamic system simulation.
Availability of the Text and Computer Code
While the text and associated computer code are copyrighted, they are made available
from the author’s account at researchgate.net, in electronic form, at no cost. The author
encourages feedback and suggestions for improvements in the text and computer code in future
releases at echaug@gmail.com.
iv
PREFACE TO THE SECOND EDITION
At the invitation of Professor Bahram Ravani of the University of California at Davis, the
author taught a 4 quarter hour (40 teleconference lecture) graduate course during the fall of 2020
based on the first edition of the text. The content of the text, excepting chapters 6 and 8 on
nonholonomic systems and systems with friction was covered, including use of embedded
computer code for class projects. As expected, many previously undetected errors in the text and
computer codes were found and corrected.
The author acknowledges contributions of the following graduate students to improving
the text and embedded computer codes: Tammer Barkouki, Derek Brangham, Zhixiang Chao,
Cory George, Jennifer Gin, Jonathan Gordon, Blake Hannah, Victor Honein, Michael Horton,
Abhay Negi, Ryan Ovenell, Diego Rojas, Ahmadreza Safavi, Caitlin Speliotopoulos, Nicholas
Taylor, Trevor Vidano, Cho Fai Wong, and Anna Zhong.
v
CONTENTS
1 ELEMENTS OF MECHANICAL SYSTEM DYNAMICS 1

1.0 Introduction 1
1.1 Scope and Typical Applications 2
1.1.1 Slider-Crank Mechanism 3
1.1.2 Fly-Ball Governor 4
1.1.3 Windshield Wiper 5
1.1.4 Automobile Suspension 5
1.1.5 Serial Robotic Manipulator 7
1.1.6 Front-End Loader 8
1.2 Objectives and Foundations of Computational Dynamics 10
1.2.1 Emergence of the Discipline of Computational Dynamics 10
1.2.2 Basic Elements of Mechanical System Dynamics 10
1.2.3 Foundations for Computational Mechanical System Dynamics 15
2 POSITION AND ORIENTATION OF BODIES 17

2.0 Introduction 17
2.1 Vector Analysis 19
2.1.1 Geometric Vectors 19
2.1.2 Algebraic Vectors 24
2.1.3 Vector Identities 27
2.1.4 Time Derivatives of Vectors 29
2.2 Matrix Algebra and Multivariable Calculus 33
2.2.1 Matrix Algebra 33
2.2.2 Lagrange Multiplier Theorem of Linear Algebra 38
2.2.3 Multivariable Calculus 39
2.2.4 Euclidean Space, Continuity of Functions, and Topologies 40
2.2.5 Implicit and Inverse Function Theorems 42
2.2.6 Differential Constraints 45
2.2.7 Newton-Raphson Method for Solution of Nonlinear Equations 47
2.2.8 Matrix Condition Number 47
2.3 Position and Orientation of a Body in a Plane 50
2.3.1 Reference Frames for Location and Orientation in a Plane 50
2.3.2 Orientation Degrees of Freedom 53
2.3.3 Orientation Identities 54
2.3.4 Time Derivatives and Variations in Rotation and Displacement 56
2.4 Position and Orientation of a Body in Space 58
2.4.1 Reference Frames for Location and Orientation in Space 58
2.4.2 Transformation of Coordinates 60
2.4.3 Orientation Degrees of Freedom 63
2.4.4 Identities Involving the Orientation Transformation Matrix 66
2.4.5 Velocity, Acceleration, Angular Velocity, and Angular Acceleration 69
2.4.6 Infinitesimal Rotation and Displacement 72
2.5 Euler Parameter Orientation Coordinates for a Body in Space 75
2.5.1 Invariants of Orientation Transformation 75
2.5.2 Orientation of Reference Frames 76
2.5.3 Euler Parameters 78
2.5.4 Mapping from the Orientation Matrix to Euler Parameters 79
2.5.5 Point Definition of an Orthogonal Reference Frame 82
2.5.6 Code 2.5, Definition of A and Solution for p with Given A 84
2.6 Euler Parameter Identities and Derivatives 87
2.6.1 Euler Parameter Identities 87
2.6.2 Euler Parameter Derivatives 90
2.6.3 Time Derivatives and Variations of Euler Parameters 93
2.6.4 Euler Parameter Differentials and Infinitesimal Rotation 95
vi
2.6.5 Nonintegrability of Angular Velocity 95
2.6.6 Angular Acceleration as a Function of Euler Parameter Derivatives 96
2.6.7 Special Identities 97
2.6.8 Derivatives with Respect to Model Parameters 97
Appendix 2.A Euler Angle Orientation Coordinates 101
2.A.1 Definition of Euler Angles 101
2.A.2 Euler Angle Singularity 103
2.A.3 Complexity of Computation with Euler Angles 105
Appendix 2.B Position and Orientation Code 108
2.B.1 Code 2.2 Newton Raphson Solution of k Equations F(x) = 0 for k Variables x 108
2.B.2 Code 2.4 Computation of Relative Rotation About Common z’ -Axes 109
2.B.3 Code 2.5 Definition of A and Solution for p with Given A 110
2.B.4 Code 2.6 Euler Parameter Identities and Derivative Operators 111
Appendix 2.C Key Formulas, Chapter 2 114
3 KINEMATICS OF MULTIBODY SYSTEMS 117

3.0 Introduction 117
3.1 Holonomic Constraints 118
3.1.1 Constraints Between Pairs of Bodies 118
3.1.2 Constraint, Velocity, and Acceleration Equations 121
3.2 Planar Joints, Constraint Equations, and Drivers 125
3.2.1 Constraints 125
3.2.1.1 Distance Constraint 125
3.2.1.2 Revolute Constraint 126
3.2.1.3 Translational Constraint 127
3.2.2 Kinematic Drivers 129
3.2.3 Planar System Examples 130
3.2.3.1 Double Pendulum 130
3.2.3.2 Slider-Crank, 3-Body Model 131
3.2.3.3 Slider-Crank, 2-Body Model with Rotation Driver 132
3.2.3.4 Windshield Wiper 134
3.3 Spatial Joints, Constraint Equations, and Drivers 136
3.3.1 Structure of Constraint Equations 136
3.3.2 Building Block Constraints 137
3.3.2.1 Distance Constraint 137
3.3.2.2 Spherical Constraint 139
3.3.2.3 Dot 1 Constraint 140
3.3.2.4 Dot 2 Constraint 141
3.3.3 Cylindrical, Revolute, and Translational Joints 142
3.3.3.1 Cylindrical Constraint 142
3.3.3.2 Revolute Constraint 143
3.3.3.3 Translational Constraint 143
3.3.4 Composite Joints 144
3.3.4.1 Universal Constraint 144
3.3.4.2 Strut Constraint 148
3.3.4.3 Revolute-Spherical Constraint 148
3.3.5 Kinematic Drivers 149
3.3.6 Euler Parameter Normalization Constraints 151
3.3.7 Spatial System Examples 152
3.3.7.1 Two Body Spatial Pendulum 152
3.3.7.2 Spatial Slider-Crank 153
3.3.7.3 Automobile Suspension 155
3.4 Nonholonomic Constraints 157
3.4.1 Examples of Nonholonomic Systems 157
3.4.2 System Holonomic and Nonholonomic Constraints 162
3.5 Configuration Space and the Constraint Manifold 165
3.5.1 Configuration Space 165
vii
3.5.2 Constraint Manifolds 168
3.5.3 Tangent Space Parameterization of Constraint Manifolds 172
3.5.4 The Regular Configuration Space C% is a Differentiable Manifold 175
3.5.5 Computation of B(q) and h(v) 178
3.6 Differential Geometry of Constraint Manifolds 180
3.6.1 Euclidean Space 180
3.6.2 Differential Geometry of the Regular Constraint Manifold 181
3.6.3 Examples 183
3.7 Singular Configurations 186
3.7.1 Examples of Singular Behavior 186
3.7.2 Singularities Due to Time Dependent Drivers 188
3.7.3 Criteria for Singular Behavior 189
3.8 Kinematic Position, Velocity, and Acceleration Analysis 193
3.8.1 Generalized Coordinates and Constraint Equations 193
3.8.2 System Assembly 195
3.8.3 Position Analysis 195
3.8.4 Velocity and Acceleration Analysis 196
3.8.5 Criteria for Singular Behavior 196
3.9 Code 3.9 for Planar System Kinematic Analysis 198
3.9.1 User Components of Code 198
3.9.2 Computational Components of Code 201
3.9.3 Code Output 206
3.10 Planar System Kinematic Analysis Using Code 3.9 207
3.10.1 Planar Slider-Crank 207
3.10.2 Quick Return Mechanism 209
3.10.3 Windshield Wiper 211
3.11 Code 3.11 for Spatial System Kinematic Analysis 214
3.12 Spatial System Kinematic Analysis Using Code 3.11 224
3.12.1 Two Bar Mechanism with Two Rotation Drivers 224
3.12.2 Four Bar Mechanism with Internal Rotation Driver 228
3.12.3 Two Body Slider with Distance Driver 229
3.12.4 Slider-Crank in y-z Plane 231
3.12.5 Spatial Slider-Crank 234
Appendix 3.A Kinematics Code 238
Appendix 3.B Key Formulas, Chapter 3 239
4 EQUATIONS OF MOTION FOR BODIES AND MULTIBODY SYSTEMS 241

4.1 Equations of Motion for Particles 242
4.1.1 Newton's Laws 242
4.1.2 Virtual Displacements 243
4.1.3 d'Alembert's Principle 246
4.1.4 Variational Equations of Motion for Systems of Particles 246
4.2 Equations of Motion for Planar Rigid Bodies 252
4.3 Equations of Motion for Spatial Rigid Bodies 258
4.3.1 Angular Velocity Form of Equations of Motion 258
4.3.2 Euler Parameter Form of Equations of Motion 262
4.4 Centroid and Moments and Products of Inertia 265
4.4.1 Location of Centroid 265
4.4.2 Transformation of Inertia Matrices 267
4.4.3 Principal Axes 268
4.4.4 Transformation of Planar Polar Moment of Inertia 269
4.4.5 Inertia Properties of Complex Bodies 270
4.5 Internal Generalized Forces 276
viii
4.5.1 Internal Forces in Spatial Systems 276
4.5.2 Internal Forces in Planar Systems 279
4.6 Variational Equations of Motion for Multibody Systems 281
4.6.1 System Constraints and Virtual Displacements 281
4.6.2 Variational Equations of Motion 282
4.6.3 Positive Definiteness of System Mass Matrix 283
4.6.4 Initial Conditions 285
4.7 ODE of Motion in Independent Generalized Coordinates 287
4.7.1 Examples of Systems with Independent Generalized Coordinates 287
4.7.2 General Form of ODE of Multibody Dynamics 293
4.7.3 Theory of ODE 296
4.8 Runge-Kutta Methods for Numerical Solution of ODE 298
4.8.1 Runge-Kutta Methods for First Order ODE 298
4.8.1.1 Explicit Runge-Kutta Methods for First Order ODE 300
4.8.1.2 Implicit Runge-Kutta Methods for First Order ODE 302
4.8.2 Runge-Kutta Methods for Second Order ODE 304
4.8.2.1 Explicit Runge-Kutta Methods for Second Order ODE 307
4.8.2.2 Implicit Runge-Kutta Methods for Second Order ODE 310
4.8.3 Generalized Form of Second Order ODE 311
4.8.4 Second Order Trapezoidal Method 312
4.8.5 Third Derivatives for Variable Step Size Error Control 312
4.9 Numerical Solution for Systems Governed by ODE 315
4.9.1 Code 4.8 for Second Order ODE Solution 315
4.9.2 Numerical Examples of Systems Governed by ODE 315
4.9.2.1 Planar Double Pendulum 315
4.9.2.2 Planar Slider-Crank 319
4.9.2.3 Spatial Robotic Manipulator 321
4.10 Constraint Forces and Differential-Algebraic Equations of Motion 327
4.10.1 Generalized Constraint Reaction Forces 327
4.10.2 Differential-Algebraic Equations of Motion 328
4.10.3 Joint Constraint Reaction Force and Torque 329
4.10.4 Evaluation of Constraint Reaction Forces 331
Appendix 4.A ODE Solution Code 332
4.A.1 Code 4.8 Second Order ODE Solvers 332
4.A.1.1 User Components of Code 4.8 332
4.A.1.2 Computational Components of Code 333
4.A.2 Code 4.A.1 Planar Double Pendulum 335
4.A.3 Code 4.A.2 Planar Slider-Crank 335
4.A.4 Code 4.A.3 Three Degree of Freedom Robotic Manipulator 335
5 TANGENT SPACE ODE FOR HOLONOMIC SYSTEMS 338

5.1. Model Problems That Require Local Coordinates 340
5.1.1Particle on Unit Sphere 340
5.1.1.1 Tangent Space Generalized Coordinates 340
5.1.1.2 ODE of Motion 343
5.1.1.3 Numerical Solution 345
5.1.2 Symmetric Top 345
5.1.2.1 Tangent Space Generalized Coordinates 346
5.1.2.2 ODE of Motion 348
5.1.2.3 Numerical Solution 350
5.2 Tangent Space ODE 353
5.2.1 Tangent Space Kinematics with Time Dependent Constraints 353
5.2.2 Kinematic Relations 356
5.2.3 Tangent Space Kinematics with Time Independent Constraints 358
ix
5.2.4 ODE of Motion 359
5.2.5 Calculation of Constraint Reaction Forces 361
5.3 Numerical Solution of Tangent Space ODE 362
5.3.1 Explicit Numerical Integration Algorithm 362
5.3.2 Derivatives for Implicit Integration 364
5.3.3 Organization of Derivative Calculation 367
5.3.4 Trapezoidal Implicit Numerical Integration 369
5.3.5 Runge-Kutta Implicit Numerical Integration 370
5.3.6 Implicit Numerical Integration Algorithm 371
5.4 Numerical Examples with Tangent Space ODE 373
5.4.1 Planar Double Pendulum 373
5.4.2 Top with Tip Fixed 376
5.4.2.1 Spin Stabilized Top 376
5.4.2.2 Transient Top 379
5.4.3 Top with Tip Sliding on x-y Plane 380
5.4.4 Spatial Double Pendulum 385
5.4.5 Need for Automated Equation Formulation and Solution 390
5.5 Tangent Space Index 0 DAE 391
5.5.1 Index 0 DAE Formulation 391
5.5.2 Explicit Integration of Index 0 DAE 393
5.5.3 Derivatives for Implicit Integration of Index 0 DAE 394
5.5.4 Implicit Trapezoidal Integration of Index 0 DAE 395
5.5.5 Implicit Runge-Kutta Integration of Index 0 DAE 396
5.5.6 Implicit Index 0 DAE Integration Algorithm 398
5.5.7 Third Derivatives for Error Control 398
5.6 Numerical Examples with Tangent Space Index 0 DAE 400
5.6.1 Top with Tip Fixed 400
5.6.1.1 Spin Stabilized Top 400
5.6.1.2 Transient Top 401
5.7 Code 5.7 for Tangent Space Simulation of Planar Systems 403
5.8 Planar System Simulation Using Code 5.7 412
5.8.1 Double Pendulum 412
5.8.1.1 Model and Data Set 412
5.8.1.2 Nominal Results 413
5.8.1.3 Results for Stiff Systems 413
5.8.1.4 Results for Highly Oscillatory Systems 416
5.8.1.5 Level of Effort Required 417
5.8.2.2 Simulation Results and Analysis 419
5.8.2.3 Level of Effort Required 421
5.8.3 Surge Waves in a Coil Spring 422
5.8.4 Three-Body Mechanism with Translational Joints in Ground 425
5.8.5 Slider-Crank 427
5.8.6 Rotating Disk with Translating Body 429
x
5.9 Code 5.9 for Tangent Space Simulation of Spatial Systems 433
5.10 Spatial System Simulation Using Code 5.9 441
5.10.1 Spin Stabilized Top 441
5.10.3 Body in Cylindrical Joint with Ground 445
5.10.4 Spatial Slider-Crank 447
5.10.5 Four Body Mechanism with Translational Joints in Ground 450
5.11 Well Posed Formulations of Holonomic Mechanical System Dynamics 455
5.11.1 Introduction 455
5.11.2 Design Dependence in Variational Equations of Motion 455
5.11.3 Initial Conditions as Functions of Problem Data 456
5.11.5 Well Posed ODE and Index 0 DAE of Motion 461
5.11.6 Well Posed Variational Equations of Motion 462
5.11.7 Well Posed Full DAE of Motion 463
Appendix 5.A Holonomic Tangent Space Code 467
Appendix 5.B Kinematic and Kinetic Derivatives 468
5.B.1 Derivatives of Kinematic Constraints 468
5.B.1.1 Derivatives of Planar Constraints 468
5.B.1.2 Derivatives of Spatial Constraints 471
5.B.2 Derivatives of Kinetic Quantities 475
5.B.2.1 Derivatives of Planar Kinetics 475
5.B.2.2 Derivatives of Spatial Kinetics 476
5.B.3 Derivatives of Generalized Forces 477
5.B.3.1 Derivatives of Planar Generalized Forces 477
5.B.3.2 Derivatives of Spatial Generalized Forces 480
Appendix 5.C Key Formulas, Chapter 5 484
Appendix 5.D The Trouble with Maggi and Kane Equations 486
5.D.1 Kinematics Formulation 486
5.D.2 Maggi/Kane Equations with Only Velocity and Acceleration Constraints 487
5.D.3 Inaccuracy of Maggi and Kane Equations for Holonomic Constraints 489
Appendix 5.E Generalized Coordinate Partitioning 490
5.E.1 Gaussian Elimination with the Constraint Jacobian Matrix 490
5.E.2 The Differential-Algebraic Equations of Motion 491
5.E.3 Partitioning the DAE of Dynamics 492
6 TANGENT SPACE ODE FOR NONHOLONOMIC SYSTEMS 496

6.1 Tangent Space Kinematics for Nonholonomic Systems 497
6.1.1 Nonholonomic System Kinematics 497
6.1.2 Configuration Tangent Space Parameterization 498
6.1.3 Velocity Tangent Space Parameterization 500
6.1.4 Kinematic Differential Equation 501
6.1.5 Kinematically Admissible Virtual Displacements 502
6.2 Tangent Space ODE for Dynamics of Nonholonomic Systems 504
6.2.2 Explicit Numerical Integration 505
6.2.3 Derivatives for Implicit Numerical Integration 506
6.2.4 Implicit Numerical Integration Algorithms 508
6.2.4.1 Trapezoidal Integration 508
6.2.4.2 SDIRK Integration 510
6.2.4.3 Implicit Integration Algorithm 511
6.3 Numerical Examples with Nonholonomic ODE 512
xi
6.3.1 Planar Three Wheel Transporter 512
6.3.2 Planar Articulated Vehicle 516
6.3.3 Spatial Disk Rolling Without Slip on x-y Plane 519
6.3.4 Spatial Ellipsoid Rolling Without Slip on a Moving Surface 522
6.4 Index 0 DAE Formulation for Nonholonomic Systems 531
6.4.1 Index 0 DAE for Nonholonomic Systems 531
6.4.2 Explicit Integration of Nonholonomic Index 0 DAE 533
6.4.3 Derivatives for Implicit Numerical Integration 533
6.4.4 Implicit Numerical Integration Algorithms 534
6.4.4.1 Trapezoidal Integration 534
6.4.4.2 SDIRK Integration 535
6.4.4.3 Implicit Integration Algorithm 537
6.5 Numerical Examples with Index 0 DAE 538
6.5.1 Planar Articulated Vehicle 538
6.5.2 Spatial Disk Rolling Without Slip on x-y Plane 540
6.6 Well Posed Formulations of Nonholonomic Mechanical System Dynamics 544
Appendix 6.A Tangent Space Nonholonomic Code 548
7 DAE METHODS FOR HOLONOMIC SYSTEMS 550

7.1 The DAE of Holonomic Systems 551
7.1.1 Lagrange Multipliers 551
7.1.2 Index of DAE 552
7.1.3 An Alternative DAE Formulation 553
7.1.4 The Petzold Imperative; DAE are not ODE 554
7.2 Formulations for Numerical Solution of DAE 555
7.2.4 Correction of Constraint Errors 556
7.2.5 Numerical Approaches to Solution of DAE 558
7.2.6 Third Derivative for Error Control 559
7.3 Numerical Integration of the DAE of Holonomic Systems 561
7.3.1 Integration of Index 1 DAE 561
7.3.1.1 Explicit Integration of Index 1 DAE 561
7.3.1.2 Implicit Integration of Index 1 DAE 561
7.4 Code 7.4 for DAE Simulation of Planar Systems 566
7.5 Planar System Simulation Using DAE Code 7.4 571
7.6 Code 7.6 for DAE Simulation of Spatial Systems 579
7.7 Spatial System Simulation Using DAE Code 7.2 583
7.7.1 Spin Stabilized Top 583
Appendix 7.A DAE Solution Code 587
8 SIMULATION OF FRICTION IN MULTIBODY DYNAMICS 589

xii
8.1 Modeling Friction in Multibody Dynamics 590
8.1.1 Static Coulomb Friction in a Constrained System 590
8.1.2 Three Mass Planar Model with Friction and Stiction 591
8.1.2.1 Mechanism Model 591
8.1.2.2 Continuous Friction-Slip Velocity Function 592
8.1.2.3 Tangent Space Index 0 Model with Friction 594
8.1.2.4 Explicit Integration of the Index 0 DAE 595
8.1.2.5 Implicit integration of the Index 0 DAE 596
8.1.3. Simulation of Three Mass planar Mechanism 597
8.2 Analytical Stiction Criteria and Ranges of Stiction Forces 601
8.2.1 Analytical Stiction Criteria for the Three Mass Model 601
8.2.2 Analytical Solution of Stiction Inequalities 604
8.2.3 Admissible Ranges of Stiction Forces 606
8.2.4 Numerical Solution of Coulomb Stiction Criteria 608
8.3. Cartesian Coordinate Simulation of Model Problems 611
8.3.1 Cartesian Coordinate Simulation of a Three Mass System 611
8.3.2 Cartesian Coordinate Simulation of a Four Mass System 613
8.4 Modeling Planar Multibody Systems with Friction 618
8.4.1 Constraint Reaction Forces and Friction Generalized Forces 618
8.4.1.1 Revolute Joint Reaction and Friction Forces 619
8.4.1.2 Translational Joint Reaction and Friction Forces 620
8.4.2 Derivatives of Friction Generalized Forces 621
8.5 Modeling Spatial Multibody Systems with Friction 624
8.5.1 Constraint Contact Reaction Forces 624
8.5.2 Friction Forces in Joints 627
8.5.3 Derivatives of Friction Forces 630
8.6 Index 0 DAE Formulation for Systems with Friction 636
8.6.1 Index 0 Equations of Motion 636
8.6.2 Explicit Numerical Integration Algorithm 636
8.6.3 Implicit Numerical Integration Algorithm 638
8.7 Code 8.7 for Simulation of Planar Systems with Friction 641
8.8 Planar System Simulation with Friction Using Code 8.7 645
8.8.1 Three slider Mechanism 645
8.8.4 Surge Waves in a Coil Spring 652
8.9 Code 8.9 for Simulation of Spatial Multibody Systems with Friction 658
8.10 Spatial System Simulation with Friction Using Code 8.9 662
8.10.1 Body in Cylindrical Joint with Ground 662
8.10.3 Four Body Slider 667
Appendix 8.A Tangent Space Index 0 DAE Friction Code 674
9 MANIPULATOR KINEMATICS AND DYNAMICS 676

9.1 Characteristics of Manipulators 677
9.1.1 Serial Manipulators 677
9.1.2 Spatial Stewart Platform Mnipulator 678
9.1.3 Planar Stewart Platform Analog 679
9.1.4 Tractor Front-End Loader 679
xiii
9.1.6 Relation of Formulation Presented to the Literature 681
9.2 Parallel Manipulator Coordinates and Local Relationships 683
9.2.1 Manipulator Coordinates and Kinematic Equations 683
9.2.2 Slider-Crank Illustration 684
9.2.3 Forward and Inverse Kinematics 685
9.3 Singularity Free Domains of Functionality 689
9.3.1 Differentiable Manifolds of Manipulator Functionality 689
9.3.2 Singularity Free Domains of Functionality 690
9.3.3 Domains of Functionality with Input Bounds 692
9.3.4 Domains of Functionality for the Slider-Crank Manipulator 693
9.3.4.1 Domains of Functionality with l > 1 694
9.3.4.2 Domains of Functionality with l = 1 696
9.4 Domains of Functionality for a Planar Manipulator 698
9.5 Front-End Loader 704
9.5.1 Loader Kinematics 704
9.5.2 Domains of Functionality 706
9.5.3 Domains of Functionality with Input Bounds 708
9.6 A Model Manipulator 711
9.6.1 Manipulator Coordinates and Equations 711
9.6.2 Domains of Functionality 713
9.6.3 Boundaries of Domains of Functionality as Sets of Singularities 714
9.7 Computation of Domain Boundaries 717
9.7.1 Criteria for Boundaries of Domains of Functionality 717
9.7.2 Approximating Boundaries of Domains of Functionality 717
9.7.3 Boundaries of Domains of Functionality for Model Problem of Section 9.6 718
9.8 Manipulator Kinematic Analysis 720
9.8.1 Multivariable Differential Calculus 720
9.8.2 Forward and Inverse Configuration Mappings 722
9.8.3 Forward and Inverse Velocity Mappings 723
9.8.4 Computation of Forward and Inverse Kinematics 724
9.8.5 Real-Time Computation 726
9.9 ODE of Manipulator Dynamics Embedded in Domains of Functionality 727
9.10 Parallel Manipulator Equations of Motion 730
9.10.1.1 Derivatives Required for Equations of Motion 730
9.10.1.2 Equations of Motion for the Front-End Loader 731
9.10.2 Stewart Platform 732
9.10.2.1 Stewart Platform Kinematics 733
9.10.2.2 Stewart Platform Equations of Motion 736
Appendix 9.A Key Formulas, Chapter 9 738
Bibliography 739
Index 748
xiv
CHAPTER 1
Elements of Mechanical System Kinematics and Dynamics
1.0 Introduction
Knowledge related to dynamics of particles and celestial bodies was dramatically
advanced by Newton in the 17th century (Newton, 1687), with the formulation of fundamental
laws of motion of discrete masses and a foundation for calculus that enables quantification of the
laws. A century later, d’Alembert (1743), Euler (1765), and Lagrange (1788) created a sound
analytical foundation for the field of mechanical system dynamics that survives to the present
time. In the subsequent quarter of a millennium, the brightest mathematical and scientific minds
added to the foundation of knowledge and methods for obtaining governing equations of motion
and characterizing properties of their solutions. Extreme nonlinearity and high dimensionality
that characterizes the equations of mechanical system kinematics and dynamics, however,
provides an insurmountable barrier to analytic solution of problems of immense scientific and
engineering importance.
In the last third of the 20th century and first two decades of the 21st century, technology
has emerged that makes simulation and analysis of the kinematics and dynamics of mechanical
systems possible for applications of contemporary impoortance. Not surprisingly, this
development parallels almost precisely revolutionary advancements in digital computer
technology and computational science, whose impact on kinematics and dynamics of mechanical
systems has been nothing short of spectacular. Kinematics and dynamics of mechanical systems,
sometimes called multibody systems, has advanced to the point that it is accepted as a discipline
unto itself, with several journals devoted to research contributions. Thousands of papers and
dozens of books on the subject have been published during the past four decades. Texts on
computational aspects of mechanical system kinematics and dynamics, however, are accessible
primarily to advanced graduate students and research engineers. In parallel, a rich mathematical
and numerical analysis literature and applicable software base has evolved that provides the
foundation for computational kinematics and dynamics. Unfortunately, some of the most
important contributions are presented at a mathematical level that is beyond the comfort level of
we mere mortal engineers.
The text Computer Aided Kinematics and Dynamics of Mechanical Systems, Volume 1:
Basic Methods (Haug, 1989) was based on the state of computational kinematics and dynamics
technology in 1989. The objective of this text is to present modern foundations that have evolved
for computational formulation and solution of the equations of kinematics and dynamics of
mechanical systems. These results enable graduate students to master basics of the subject, carry
out practical applications, and access the extensive literature and application software base that
has evolved over time.
The purpose of this chapter is to (1) define the scope of the subject treated, (2) outline
typical applications that illustrate capabilities desired, and (3) identify objectives, foundations,
and challenges that face computer formulation and solution of the governing equations of
mechanical system kinematics and dynamics.
1
1.1 Scope and Typical Applications
A mechanical system, also called a multibody system, is defined as a collection of
interconnected bodies that can move relative to one another, consistent with joints that limit
relative motion of pairs of bodies. Typical mechanical systems are vehicles, machine tools,
agricultural and construction equipment, robots, and manipulators. Initial conditions, internal
forces due to friction and control inputs, and externally applied forces may be specified, in which
case dynamics of the system is determined by equations of motion that are based on the
geometry of kinematics and laws of dynamics. Kinematics and dynamics of mechanical systems
are characterized by large amplitude motion, which leads to geometric nonlinearity that is
reflected in algebraic equations of constraint and differential equations of motion. Nonlinearity
of mechanical system kinematics and dynamics is often viewed as an annoyance. It must be
realized, however, that some of the most valuable contributions made by mechanisms and
machines to modern technology are due to their nonlinearity. The slider-crank mechanism of
Section 1.1.1 that is the foundation for numerous machines is highly nonlinear, transforming
reciprocating motion of the slider to rotational motion of the crank. Without it, the internal
combustion engine that has powered transportation and the broader industrial revolution would
not be possible.
An important consideration that serves to classify mechanical systems concerns the
source of forces that act on the system. This is particularly important for mechanical systems on
which some form of control is exerted. Force effects due to electronic and hydraulic feedback
control play a crucial role in the dynamics of modem mechanical systems. The scope of
mechanical system dynamics is, therefore, heavily dependent on the classes of force systems that
act. The most elementary form of force that acts on a mechanical system is gravity, which is
normally taken as constant and acting perpendicular to the surface of the earth. Other forces that
act on bodies in a system, due to interaction with their environment, include aerodynamic,
friction, and damping forces that are due to relative motion of components of the system.
Another important class of forces that act in a mechanical system is associated with compliant
components such as coil springs, leaf springs, tires, shock absorbers, and a multitude of other
deformable components that create forces and torques that act on the system.
It must be noted that most mechanical systems involve some form of energy dissipation.
This is characteristic of systems with friction or hydraulic dampers, such as shock absorbers in
vehicles that are designed to control motion of the system. Feedback control systems that act in
mechanical systems involve energy input to the system. All this is in conflict with much of the
classical literature on dynamics that treats dissipation or addition of energy as an anomaly that is
outside the scope of the subject. Much of this literature is thus not applicable to systems treated
in this text.
While components of mechanical systems deform under the action of forces imposed on
them, in many applications the effects of deformation are negligible and bodies can be
considered to be rigid. Most applications addressed in this text involve bodies that are considered
to be rigid, called rigid multibody systems. Important applications in which deformation must be
accounted for are beyond the scope of this introductory text, but rely on the foundations
developed herein.
Finally, perhaps the greatest challenge facing effective modeling and simulation of
mechanical system kinematics and dynamics is the dimensionality of equations involved. For k
2
bodies that move in space, subject to m equations of constraint, the number of variables that
define the configuration of the system is 7k. With dof = 7k – m degrees of freedom, on the order
of 10 and k on the order of 10, approximately m = 7k − dof = 70 − 10 = 60 nonlinear equations of
constraint must be dealt with. This is a modest dimensionality by modern standards. This means
that even if 10 independent coordinates can be found in a change of variable to define 10
ordinary differential equations of motion, 60 variables must be eliminated from 60 highly
nonlinear equations of constraint. The result is systems of equations that cannot be practically
written in analytical form, much less solved analytically. The logical conclusion is that, since
equations of kinematics for realistic systems cannot be analytically written and solved, methods
must be created and implemented for digital computer assembly and solution of the equations of
mechanical system kinematics and dynamics. While progress has been made in creating this
capability, much remains to be done.
To be more concrete regarding classes of mechanical systems to be addressed, it is
helpful to review a few typical engineering applications. Since the audience for the text is
familiar with and relies upon self-propelled vehicles, most applications illustrated in this section
are from vehicle mechanics.
1.1.1 Slider-Crank Mechanism
The engine shown in Fig. 1.1.1 contains many moving parts and illustrates a number of
the most common mechanisms employed in machine design. The crankshaft of the engine rotates
in lubricated bearings and contains rotational bearings with connecting rods, which are coupled
through rotational bearings to pistons that reciprocate in combustion cylinders. The
crankshaft-connecting rod-piston assembly comprises what is commonly called a slider-crank
mechanism, which is used to convert reciprocating motion of the piston to rotation of the
crankshaft, a nonlinear relationship.
Figure 1.1.1 Cross Section of a V-2, 4, 6, or 8 Engine
3
Using models of combustion forces in the cylinder and fuel feed, equations of motion
may be formulated and solved for rotational speed vs. torque delivered, as the engine interacts
with the transmission. Reaction forces on bearings are determined by the equations of dynamics
to support design of bearings and other moving components. Virtually all these relationships are
nonlinear.
1.1.2 Fly-Ball Governor
A second example is the traditional fly-ball governor shown in Fig. 1.1.2. Relatively
massive balls are attached to arms that are pivoted on a rotating shaft, so that they rotate with the
shaft. Couplers are attached between the ball arms and a collar that is constrained to translate
along the shaft. The entire mechanism rotates with angular velocity ω of the shaft. As ω
increases, centrifugal forces act on the balls to throw them out, causing the collar to move
upward, hence increasing the collar height s shown in Fig. 1.1.2. The purpose of the fly-ball
governor is to control the operating speed of an engine. A mechanism couples the position s of
the collar to the fuel feed of an internal combustion engine that drives the shaft. The mechanism
is designed so that, at the desired speed of the engine, centrifugal forces on the balls and
gravitational and spring forces that act on the mechanism reach a steady state, with the collar at a
constant height. If an increased load is encountered that reduces engine speed, hence the angular
velocity ω of the shaft; e.g., a vehicle encountering a hill or a lawn mower encountering tall
grass, the balls will drop and the collar will move downward. The mechanism that couples the
position s of the collar with the fuel intake provides additional fuel, which in turn speeds the
engine and causes centrifugal forces on the balls to increase, raising the balls toward their
nominal height and returning the angular velocity of the shaft to its nominal value. This governor
mechanism gives rise to the expression that the associated machine is going “balls out” when the
engine runs at top speed. Motion of the mechanism and centrifugal force that enables the system
to function as desired depend on numerous nonlinear effects. If machine dynamics were linear,
this system could not perform its function.
x
Figure 1.1.2 Fly-Ball Governor
4
1.1.3 Windshield Wiper
The windshield wiper mechanism shown in Fig. 1.1.3 is an application of four-bar
linkages that transmit motor-driven rotation of the crank to reciprocating motion of the
windshield wipers, a nonlinear relation. The crank and left rocker arm are pivoted in the vehicle
frame at points A and B. A coupler is attached to the crank at point C and to the left rocker arm
at point D. The crank, crank coupler, left rocker, and frame of the vehicle constitute a four-bar
linkage. Since the distance from B to D is greater than the distance from A to C, a full rotation of
the crank causes only a partial rotation of the left rocker arm, leading to the desired reciprocating
motion of the left windshield wiper. The dimensions of the various links are selected to generate
the desired range of motion. A second four-bar linkage is formed by the right rocker arm that is
pivoted in the frame of the vehicle at point G and a coupler that is connected to the left and right
rocker arms at points E and F. This second linkage transmits reciprocating motion from the left
rocker arm to the right rocker arm, hence controlling motion of the right windshield wiper. As
with the slider-crank mechanism and the fly-ball governor, this mechanism depends on nonlinear
geometric effects to perform its function. Its dynamic performance is effected by inertia of the
moving components, motor torque delivered, and hydrodynamic and friction forces between the
wiper blades and windshield.
Figure 1.1.3 Windshield Wiper Mechanism

1.1.4 Automobile Suspension
The final vehicle related example, which illustrates the scope of applications that are
addressed in this text, is the vehicle of Fig. 1.1.4, whose suspension system is shown
schematically in Figs. 1.1.5 and 1.1.6. This commonly employed suspension consists of a
McPherson strut front suspension and a trailing arm rear suspension. Each front wheel assembly
is attached to the chassis of the vehicle through a lower control arm and a telescoping strut-
spindle assembly, as shown in Fig. 1.1.6. Concentric with the strut are suspension springs and
shock absorbers. Ball joints at the top and bottom of the strut permit steering rotation of the
wheel assembly about the strut, controlled by tie rods and the steering rack. The more
elementary rear suspension shown in Fig. 1.1.5 is simply a trailing arm that is pivoted in the
chassis to permit the rear wheel assembly to move relative to the chassis. Spring and shock
absorber components that act between the trailing arm and chassis provide for support of the
chassis and cushioning of nonlinear tire-road interaction force effects.
5
Dynamic analysis is carried out, using models of forces due to tire-road interaction,
spring and shock absorber forces, forces at the driven wheel spindles due to engine torque, and
steering rack displacement due to driver actions, to determine motion of the vehicle on the
roadway. Many such analyses are carried out in designing subsystems of a vehicle. The nonlinear
behavior of this intricate mechanical system enables vehicle performance characteristics that
would not be possible if it were linear. As will be shown in the text, with wheel rotation, system
kinematics are governed by 84 variables, called generalized coordinates, and 70 constraint
equations. The vehicle system thus has 14 degrees of freedom. Only the most wild-eyed optimist
could hope for analytical solutions of the nonlinear equations of kinematics and dynamics of
such as system. This and other examples discussed here require the power of the modern digital
computer and supporting software to both assemble and solve the equations of motion.
Figure 1.1.4 Automobile
Figure 1.1.5 Automobile Suspension Schematic
6
Figure 1.1.6 McPherson Strut Front Suspension
1.1.5 Serial Robotic Manipulator
The robot, or manipulator, shown in Fig. 1.1.7 is made up of nine bodies, including
ground (body 1). The first degree of freedom is rotation q1 of the base (body 2) about a vertical
axis fixed in ground. The second degree of freedom is rotation q 2 of the pivot arm (body 3)
about a horizontal axis fixed in body 2. The third degree of freedom is translation q 3 of the boom
(body 4) in a guide that is fixed in body 3. The fourth degree of freedom is rotation q 4 of the
first wrist pivot (body 5) relative to body 4. The fifth degree of freedom is rotation q 5 of the
second wrist pivot (body 6) relative to body 5. The sixth degree of freedom is rotation q 6 of the
hand mechanism (body 7) relative to body 6. The seventh degree of freedom is relative rotation
q 7 of the robot fingers (bodies 8 and 9). Such a mechanism permits the end-effector to grasp and
manipulate a workpiece. As in previous examples, equations of kinematics and dynamics are
nonlinear, enabling the robot to do things a linear system could not.
7
Figure 1.1.7 Serial Robotic Manipulator
This is a serial manipulator, in the sense that the sequence of inputs q1 , ,q 7 explicitly
determines the position and orientation of an outboard body, relative to its inboard body. This
greatly simplifies the process of modeling and control of the robot. Considerations specific to a
robot concern definition of admissible configurations; e.g., extreme position and orientation of
the end-effector. Limits on configuration are determined by singularities in kinematic equations
that define position and orientation of the end-effector, a specific form of nonlinearity that must
be dealt with.
1.1.6 Front-End Loader
The front-end loader shown in Fig. 1.1.8 is typical of high load capacity manipulators
used in construction. The mechanism has three bodies that move relative to body 0 (the chassis),
whose positions and orientations are defined in the chassis-fixed x-y frame by three generalized
coordinates shown in Fig. 1.1.8, q = q1 q 2 q 3  . Hydraulic actuator inputs y =  y1 y2 
T T
shown control motion of the system, and outputs are the orientation and elevation of the bucket,
T
z = q1 + q 3 y D2  . Body 2 slides along the axis of body 1 (boom) and is connected to point D1
on body 3 (bucket) by a bar of length 2 m, to control orientation of the bucket.
8
Figure 1.1.8 Front-End Loader
The mechanism of this manipulator contains a closed kinematic loop involving the slider,
boom, bucket, and bar between the slider and bucket. This is called a parallel manipulator. It is
more efficient in controlling motion under the influence of large loads than a serial manipulator,
but the associated kinematics are much more complex. More specifically, if the lines C1D1 and
D1B2 are colinear, the kinematic equations are singular. In case input q 2 is at its minimum
value, the singularity precludes a decrease in q 2 and an increase in q 2 leads to one of two
indeterminate rotations of the bucket. This form of kinematic singularity is highly nonlinear and
must be dealt with in defining limits of functionality of the manipulator.
To assist the reader in highlighting key results from among often extensive derivations, a
summary of each section is provided, as follows:
Examples presented represent typical machines and machine elements that are encountered in
mechanical system kinematics and dynamics. The breadth of such applications is extensive. A
key feature that is pervasive in mechanical system kinematics and dynamics is nonlinearity in
geometric and dynamic effects that must be accounted for. While applications differ greatly,
many technical similarities permit the development of a uniform approach to computational
kinematics and dynamics. Such an approach is presented in subsequent chapters of this text.
While the form of nonlinearity in kinematics and dynamics of the systems discussed in this
section will not be explicitly seen until Chapters 3 and 4, it is clear that output of these machines
is not a linear function of input. Nonlinearity in machine kinematics and dynamics is both a curse
and a blessing. Complexity of nonlinear systems is challenging, but is unavoidable in machines
that exploit nonlinearity for the benefit of mankind.
9
1.2 Objectives and Foundations of Computational Kinematics and Dynamics
The central objective of computational kinematics and dynamics is to create a
formulation and associated digital computer implementation that (1) derive governing equations
of kinematics and dynamics; (2) create numerical algorithms that construct and solve the
equations with specified levels of precision, and (3) create digital computer software that allows
the engineer to input data that define a mechanical system of interest, specify simulation inputs,
specify forms of output desired, and carry out simulations. The essence of this objective is to
make maximum use of digital computer power for rapid and accurate data manipulation, model
creation, and numerical computation, hence relieving the engineer of tedious and error-prone
calculations.
1.2.1 Emergence of the Discipline of Computational Dynamics
The formulation of these objectives emerged from communications among an emerging
community of engineers and scientists who contributed to their realization in the 1970s and
1980s. The first major gathering of this community in 1977 was a one week symposium entitled
“Dynamics of Multibody Systems” (Magnus, 1978). A second gathering in 1983 was a two week
NATO Advanced Study Institute on “Computer Aided Analysis and Optimization of Mechanical
System Dynamics” (Haug, 1984). A third was a symposium in 1985 entitled “Dynamics of
Multibody Systems” (Bianchi and Schiehlen, 1986). The fourth early symposium was in 1992,
entitled “Advanced Multibody System Dynamics: Simulation and Software Tools” (Schiehlen,
1993). A noteworthy series of undocumented workshops on multibody simulation methods and
software, involving leaders in mechanical and aerospace system dynamics, was hosted by the
German Aerospace Establishment at Oberphaffenhofen in the late 1980s and early 1990s. While
many professional research symposia have since occurred, these five encounters of leaders in the
emerging field of multibody dynamics were transformative. They brought together specialists
that were addressing challenging applications in mechanical engineering, aerospace engineering,
numerical analysis, and computational science, who were often unaware of each other’s work.
The result was a cross fertilization that rarely occurs across diverse disciplines and, in this case,
set off an eruption in the emerging field of multibody dynamics.
As became clear in the foregoing symposia and workshops, care must be taken to
consider numerous alternatives that are available in selecting formulations and numerical
methods to achieve significant advances in multibody dynamics. Relative and absolute
coordinate formulations were developed in the last quarter of the 20th century, using topological
analysis, symbolic computation, sparse matrix computation, and a variety of numerical
integration methods to automate the process of assembling and solving equations of motion.
Computer codes that implement the methods, some of which are now available as commercial
products, are outlined in a handbook and a monograph (Schiehlen, 1990; 1993).
1.2.2 Basic Elements of Mechanical System Dynamics
To be specific regarding some of the issues that must be addressed in creating rigorous
and practical methods that enable computer implementation of computational dynamics, an
elementary example is studied. The goal of this exercise is to illustrate some of the challenges
that must be overcome to achieve the objectives outlined in the foregoing.
10
______________________________________________________________________
Example 1.2.1: Particle on Unit Sphere. A particle of mass m is constrained to move on a unit
sphere, as shown in Fig. 1.2.1. Denoting the coordinates of the particle in the x-y-z inertial
reference frame as the 3 1 matrix q = q1 q 2 q 3  =  x y z , where superscript T denotes
T T
matrix transpose, the kinematic constraint that the particle moves on the surface of the unit
sphere is
(q) = ( qTq − 1) / 2 = ( q12 + q 22 + q 32 − 1) / 2 = 0 (1.2.1)
Defining the constraint Jacobian as the 3 1 matrix

q (q) =  / q1  / q 2  / q 3  = q1 q 2 q 3  = qT (1.2.2)
and using the chain rule of differentiation, associated velocity constraints and acceleration
constraints are
qq = 0
(1.2.3)
qq = −qq = −qTq = ( q12 + q 22 + q 32 )
where an over dot denotes derivative with respect to time; i.e., q = dq /dt . A virtual displacement
q = q1 q 2 q 3  is defined to be independent of time and to satisfy the linearized form of
T
Eq. (1.2.1),
qq = 0 (1.2.4)
Figure 1.2.1 Particle on Unit Sphere

The d’Alembert form of the variational equations of motion (see Section 4.1.3), called
the fundamental equation by Pars (1965), is
qT ( mq + mguz ) = 0 (1.2.5)
which must hold for all q that satisfy Eq. (1.2.4), where u z = 0 0 1 is the unit vector in
T
the positive z direction and no constraint reaction forces are included. Note that Eq. (1.2.5) is
much more than a statement qT ( ma − F ) = 0 . It is based on d’Alembert’s principle (Lanczos,
1962; Pars, 1965), which states that forces of constraint do no work under the action of
kinematically admissible virtual displacements.
11
There are two fundamentally different approaches to using Eqs. (1.2.4) and (1.2.5) to
obtain equations of motion. The first is to work with dependent generalized coordinates q that do
not identically satisfy the constraints of Eqs. (1.2.1) and (1.2.3) and obtain the Lagrange
multiplier form of differential-algebraic equations (DAE) of motion. The second approach is to
define independent generalized coordinates (called Lagrangian coordinates (Pars, 1965) of
dimension less than 3 that identically satisfy all three forms of the constraints of Eqs. (1.2.1) and
(1.2.3) and obtain ordinary differential equations (ODE) of motion.
The DAE Approach
In the DAE approach, since expressions in Eqs. (1.2.4) and Eq. (1.2.5) are linear in q ,
the Lagrange multiplier theorem (see Section 2.2.5) states that there exists a unique multiplier 
such that qT ( mq + mguz ) + qq = 0 for arbitrary q , or qT ( mq + ΦqT + mgu z ) = 0 . Since
q is arbitrary,
mq + qT + mguz = 0 (1.2.6)
Equation (1.2.6) is three scalar equations in the four variables q and  , the latter
appearing algebraically. This equation, together with the algebraic constraint equation of Eq.
(1.2.1) comprises a system of four differential and algebraic equations in the four variables
q and  . This system of equations is not an ODE. It has been named a DAE, since  appears
algebraically and Eq. (1.2.1) is algebraic. A landmark paper (Petzold, 1982) shows that DAE are
not ODE, and in fact are much more difficult to solve numerically.
In an attempt to overcome the deficiency of Eq. (1.2.6) as three equations in four
variables, some in the computational dynamics community append the second of Eqs. (1.2.3) to
Eq. (1.2.6) to obtain the matrix equation
 mI qT  q   − mgu z 
   =  2 
q 0     − ( q1 + q 2 + q 3 ) 
2 2
(1.2.7)
This is four equations in four variables and the determinant of the coefficient matrix of Eq.
(1.2.7), with Φq = qT and qTq = 1, is − m 2  0 , so the equation has a unique solution for
q and  . An ODE numerical integration formula (see Section 4.8) is then applied, with specified
initial conditions, to determine q and q . This computational approach suffers from two
inconsistencies. First, applying an ODE integrator to a DAE is of questionable validity (Petzold,
1982). Second, the constraints of Eq. (1.2.1) and the first of Eqs. (1.2.3) are ignored, leading to
unavoidable error in the approximate solution. Other manipulations outlined in Chapter 7 are
often attempted in solution of DAE, but still lead to difficulties.
The ODE Approach
As a second approach that avoids difficulties with DAE formulations, one may rely on
the analytical dynamics mantra (Pars, 1965) of defining independent Lagrangian generalized
coordinates that satisfy all three forms of the constraints of Eqs. (1.2.1) and (1.2.3) and apply the
d’Alembert equation of motion of Eq. (1.2.5). Using independent spherical coordinates  and 
shown in Fig. 1.2.2, the vector q is written as a function of independent coordinates,
12
cos  cos 
q =  cos  sin   (1.2.8)
 
 sin  
and their time derivatives and virtual displacements are
 − sin  cos  − cos  sin 
 
q =  − sin  sin  cos  cos    
  
 
 cos  0 
 − sin  cos  − cos  sin 

q =  − sin  sin  cos  cos     (1.2.9)
  
 cos  0 
 − sin  cos  − cos  sin   −2 cos  cos  + 2 sin  sin  − 2 cos  cos 
    
q =  − sin  sin  cos  cos     +  −2 cos  sin  − 2 sin  cos  − 2 cos  sin  
  
 cos  0     −2 sin  

Substitution of q, q, and q of Eqs. (1.2.8) and (1.2.9) into the constraints of Eqs. (1.2.1) and
(1.2.3) it is verified that all three are satisfied identically; i.e.,  and  are Lagrangian
generalized coordinates. Similarly, substituting δq of Eq. (1.2.9) into Eq. (1.2.5), that equation is
identically satisfied.
Figure 1.2.2 Spherical Coordinates for Particle on Unit Sphere

Substituting Eqs. (1.2.9) into Eq. (1.2.5) and simplifying results using trigonometric
identities yields
 1 0     2 sin  cos   cos  
   m  2    + m   + mg  0  = 0
 0 cos    −2 cos  sin    
Since  and  are arbitrary,
1 0     2 sin  cos   cos 

m 2  
= −m   − mg   (1.2.10)
0 cos    −2 cos  sin   0 
This is a pair of second order ODE in  and  . However, the coefficient matrix of the
acceleration vector is singular at  =  / 2 ; i.e., at the poles on the z axis. If one attempts to
13
numerically solve Eq. (1.2.10), a division by zero occurs during solution for  and  when
θ = π/2 . This difficulty arises for a reason quite different from those encountered when using
DAE.
In an attempt to to resolve this difficulty, define a different set of Lagrangian generalized
coordinates, such as  and  in Fig. 1.2.3 where the vector q is projected onto the x-z plane and
the Cartesian coordinates of the particle are
 cos  sin  
q =  sin   (1.2.11)
 
cos  cos  
Repeating the foregoing calculations, d’Alembert’s equations of motion in the  −  generalized
coordinates are
1 0    2 cos  sin   sin  cos 
m 2  
= − m   − mg   (1.2.12)
0 cos      −2m cos  sin  cos  sin 
In this formulation, a singularity occurs for  =  / 2 ; i.e., at the poles on the y axis. There is no
singularity at the poles on the z axis, as was the case with the  −  generalized coordinates.
Figure 1.2.3 Alternative Coordinates for Particle on Unit Sphere

The problem here is with the coordinates used to parameterize the unit sphere, not with
the smooth surface itself, which has no singularities. Topological arguments presented in
Sections 3.5 and 3.6 show that there exists no pair of generalized coordinates that can
parameterize the entire unit sphere without encountering a singularity; i.e., there exists no
globally valid pair of Lagrangian generalized coordinates. This is true even for this elementary
application, in which the surface is smooth. The moral is that if such fundamental difficulties are
encountered in this trivial application, imagine the gremlins that may lurk in real-world
applications.
To solve the equations of motion on the unit sphere, either of Eqs. (1.2.10) or (1.2.12)
may be used if the initial point is not at a pole on the z or y axes. At initial point
q((0), (0)) = q(0 , 0 ) with 0   / 2 and with initial velocities q(0 , 0 , 0 , 0 ) given by the
first of Eqs. (1.2.9), Eq. (1.2.10) may be integrated using any desired numerical integration
method. At any time t* such that (t*)  0.7(  / 2) , using q(t*) and q(t*) of Eqs. (1.2.8) and
(1.2.9), (t*), (t*), (t*), and (t*) are determined as initial conditions with which to start
14
numerical solution of Eqs. (1.2.12). The process is continued as long as (t)  0.7(  / 2) is
satisfied. If necessary, the simulation is stopped and restarted with Eqs. (1.2.10). Thus,
singularities are avoided and a smooth solution is obtained until the desired final time is reached.
The approach, however, is brute force.
____________________________________________________________________________
The point of Example 1.2.1 is that, if a system as simple as this is plagued with
difficulties, real world applications are sure to be susceptible to even more complex problems.
Further, the brute force approach to avoiding singularities in this example must be replaced by a
rigorous and systematic formulation that defines independent generalized coordinates and
formulates and solves the associated ODE of motion until analytical criteria dictate reformulation
of the equations of motion and continuation of integration. Such an approach is the basis for
differential geometry and manifolds that are introduced in advanced mathematics and physics
literature (Arnold, V. I., 1978; Dundas, 2018), and is adapted and implemented for simulation of
mechanical system dynamics in Chapter 5. This broadly applicable approach is presented, with
computer software implementation and numerical examples, in Chapter 5. Finally, it is
emphasized that a closed form analytical solution is essentially never possible in mechanical
system dynamics.
1.2.3 Foundations for Computational Mechanical System Dynamics
As illustrated by Example 1.2.1, the nonlinear nature of kinematics and dynamics of
constrained mechanical systems is fraught with subtle challenges that can only be dealt with
through a rigorous analytical foundation that can be systematically implemented in digital
computation. The trivial example of a particle on a unit sphere shows that singularities are
encountered due to selection of generalized coordinates if rational criteria are not established to
avoid their occurrence; e.g., in the form of division by zero that must be avoided at all cost in
digital computation. The classical theory of analytical dynamics, as best illustrated by the
landmark text (Pars, 1965), assumes that generalized coordinates can be defined that are globally
consistent with constraints on the system under consideration, called Lagrangian generalized
coordinates. As shown in Example 1.2.1, this is not always possible. In fact, in representing
orientation of bodies in space, even though there are three orientation degrees of freedom (see
Section 2.4), any set of three orientation generalized coordinates must have singular
configurations (Stuelpnagel, 1964). A mathematically sound foundation for computational
dynamics must therefore be established to avoid singular configurations through algorithms that
select and modify independent generalized coordinates, as necessary, during simulation. In this
sense, creation of rigorous computational formulations for mechanical system dynamics is even
more challenging than in the ad-hoc approaches used in classical analytical dynamics.
In addition to singularities that may be associated with inappropriate selection of
generalized coordinates, simulation of trial designs that exhibit physically singular behavior,
such as lock-up or bifurcation, must adapt to sense onset of such singular behavior and report it.
Condition numbers of near singular matrices may be monitored (see Section 2.2.8) and
increasing numbers of iterations required for solution of nonlinear equations (see Section 2.2.7)
may be used as computable tests of onset of singular behavior. Similarly, stiff behavior (see
Section 4.8) of the equations of dynamics for well-designed systems that use high levels of
damping to suppress unwanted high frequency oscillation lead to severe difficulties if numerical
integration methods are used that are unable to adapt to such behavior. These and related
15
numerical analysis issues that are encountered in mechanical system dynamics are compounded
in treatment of DAE formulations, as suggested in Example 1.2.1 and detailed in Chapter 7.
Extensive research in numerical analysis has been carried out since the 1980's, focused
on effective methods of solving ODE and DAE that arise in numerous fields of application.
Landmark texts that document the state of the art of numerical solution methods have been
published by Hairer and coauthors (1993; 1996). The DAE of mechanical system dynamics,
unfortunately, fall into a class called index three DAE that is difficult to solve numerically
(Ascher and Petzold, 1998). An extensive treatment of theoretical and numerical analysis of
DAE has been presented by Rabier and Rheinboldt (2002), at a level of mathematical
sophistication beyond the reach of we mere mortal engineers. Because the solution of ODE and
DAE is critical for mechanical system dynamics, and because the literature on the subject is
relatively new and tends to be mathematically abstract, attention is given in the text to basic
concepts, at a mathematical level that is accessible to the intended engineering audience.
An extensive mathematical literature on differential geometry has emerged (Spivak,
1999; Dundas, 2018) that holds enormous potential for contribution to kinematics and dynamics
of mechanical systems. Unfortunately, both the mathematical literature and its application to date
to kinematics and dynamics (Arnold, 1978; Abraham and Marsden, 1978) are at a level of
mathematical abstraction that preclude its digestion by most engineers. Basic concepts from this
literature have been adapted in this text that hopefully makes these important tools accessible to
the intended audience (see Sections 3.5 and 3.6 and Chapters 5 and 6). Recent results using these
tools to define singularity free domains of manipulator functionality and create embedded
equations of manipulator dynamics are presented in Chapter 9.
In addition to the requirement that rigorous mathematical and numerical analysis
foundations be established for computational dynamics, there must be a basic software
underpinning that permits experimentation and practical learning. The author obtained a
MATLAB license in 2015 and began relearning programming, after a 49 year period without
having written a line of code. This humbling experience has led to rudimentary MATLAB
programs that are documented in the text and contained in appendices to chapters two through
nine that implement methods presented. The reader is encouraged to use these codes to try out
ideas and designs and to report what are surely bugs and inadequacies in the programs to the
author, including suggested improvements, if possible.
Computational kinematics and dynamics emerged in the last third of the 20th century, in parallel
with evolution of the digital computer, and has become a vibrant discipline that supports
numerous areas of engineering and applied science. Realization of its full potential relies on
rigorous methods of analytical dynamics, mathematics, numerical analysis, and computational
science. Excellent texts and monographs in each of these disciplines exist, written primarily for
specialists in their discipline, but are not readily accessible to the engineer who is attempting to
advance or use the technology of mechanical system kinematics and dynamics. A goal of this text
is to bridge the gap and bring this modern technology to bear on computational methods of
16
CHAPTER 2
Position and Orientation of Bodies
2.0 Introduction
Kinematics of a multibody system defines admissible configurations and motions of
bodies in the system, consistent with joints that constrain relative motion of pairs of bodies.
Admissible configurations (positions and orientations) of bodies that make up the system are
described by vectors, matrices, and generalized coordinates that define the position and
orientation of each body in the system. A summary of basic concepts of vector analysis is
presented in Section 2.1, beginning with conventional geometric vectors and proceeding to
algebraic vectors that are comprised of components of geometric vectors relative to an
orthogonal reference frame. Algebraic vectors are shown to be ideally suited for computer
implementation, in contrast to their geometric counterparts. Time derivatives of vectors are
introduced, as the basis for velocity and acceleration analysis that is required for multibody
kinematics and dynamics.
Section 2.2 continues with a summary of basic results from matrix algebra and
multivariable calculus that form the foundation for computational kinematics and dynamics. A
Lagrange multiplier theorem of linear algebra is proved, as the basis for representing constraint
reaction forces in system dynamics. Key facts from multivariable calculus that form the
foundation for system kinematic analysis are summarized, including the implicit function
theorem that assures existence and differentiability of solutions of the nonlinear equations of
kinematics. Concepts of scalar product of n-dimensional vectors and distance between vectors
are introduced to define the Euclidean space R n . Continuity of functions in this setting is
defined, to form the foundation for differential geometry on R n , the bedrock on which
mechanical system kinematics and dynamics is built. Finally, the Newton-Raphson iterative
numerical method is presented for computer solution of nonlinear equations in Euclidean space,
with definition of matrix condition number that is used in numerical error control.
The remainder of the chapter is focused on methods for defining the position and
orientation of bodies that make up multibody systems. A characterization of orientation of bodies
that move in a plane is presented in Section 2.3, as the basis for planar system kinematics. The
foundation for defining orientation of bodies in three-dimensional space is presented in Section
2.4, in terms of orientation transformation matrices. It is shown that orientation in space has
three degrees of freedom, independent of generalized coordinates that may be used to define
orientation. Velocity and acceleration relations are derived, as the basis for spatial system
analysis. Virtual displacements are defined, using analogous relations.
Euler parameter generalized coordinates that define orientation of bodies in space and
serve as the foundation for kinematics and dynamics throughout the remainder of the text are
presented in Section 2.5. It is shown that there is locally a one to one relation between Euler
parameters and elements of the orientation transformation matrix, so Euler parameters are
singularity free. A number of identities and derivative operators involving Euler parameters are
derived in Section 2.6 that serve as the basis for kinematic and dynamic modeling and computer
simulation. These identities and operators form the basis for a library of computer subroutines in
Appendix 2.C for implementation of kinematic equations in digital computation.
17
Classical Euler angles are defined in Appendix 2.A, as three generalized coordinates that
characterize orientation of bodies in space. The orientation transformation matrix is written as a
function of Euler angles and is shown to have a singularity that must be avoided in practice.
Literature is cited showing that singularities arise for any set of three orientation generalized
coordinates. Appendix 2.B focuses on issues and progress in defining and using generalized
coordinates that effectively represent orientation of bodies in space and are well suited for digital
computation.
MATLAB computer code is provided in Appendix 2.C to implement vector and matrix
operators that are derived in the chapter. Code 2.4 evaluates the angle of relative rotation and its
derivatives between bodies in space that are constrained to admit relative rotation about a
common axis. Code 2.5 evaluates the orientation transformation matrix for a body, using vector
data or Euler’s theorem, and evaluates Euler parameters that characterize orientation for a given
orthogonal orientation transformation matrix. Code 2.6 evaluates Euler parameter derivative
operators that are derived in Section 2.6 and form the basis for numerical methods throughout the
text. Finally, a compilation of key formulas derived in Chapter 2 is presented in Appendix 2.D,
summarizing important and often used results that need not be memorized.
18
2.1 Vector Analysis
Vectors play a crucial role in locating points in the plane and in space and characterizing
geometric relationships such as orthogonality and parallelism. The conventional geometric
definition of vectors serves these purposes, but is not well suited for computation. Following a
brief summary of geometric vector analysis, an equivalent matrix form of algebraic vector
analysis that is ideally suited for computation is introduced. A reader who is not conversant in
the basics of matrices should read Section 2.2 before digesting the contents of this section.
2.1.1 Geometric Vectors
The concept of a vector in space may be introduced in a geometric setting, with no
requirement for identification of a reference frame. In this setting, a geometric vector, or simply
a vector, is defined as the directed line segment from one point in space to another point in space.
r
Vector a in Fig. 2.1.1 is a directed line segment that begins at point A and ends at point B. The
r r
magnitude of a vector a is its length (the distance between A and B) and is denoted by a, or a .
Note that the length of a vector is positive if points A and B do not coincide and is zero if and
r
only if they coincide. A vector with zero length is denoted as 0 and is called the zero vector.
r
Figure 2.1.1 Vector a From Point A to Point B
r r
Multiplication of a vector a by a nonnegative scalar α ³ 0 is defined as the vector αa in
r r
the same direction as a , but having magnitude α a . A unit vector, having length 1 unit, in the
r r r r
direction a ¹ 0 is (1 / a ) a . Multiplication of a vector a by a negative scalar β < 0 is defined as
r r
the vector with magnitude β a and direction opposite to that of a . The negative of a vector is
obtained by multiplying the vector by -1 . It is the vector with the same magnitude but opposite
direction.
________________________________________________________________________
Example 2.1.1: Let points A and B in Fig. 2.1.1 be located in an x-y-z orthogonal reference
frame shown in Fig. 2.1.2; i.e., a reference frame with orthogonal axes.
r
Figure 2.1.2 Vector a Located in Orthogonal Reference Frame
19
The distance between points A and B, with coordinates ( A x ,A y ,A z ) and ( Bx ,B y ,Bz ) ,
r
respectively, is the length of a ; i.e.,
r 1/ 2
a = é( Bx - A x ) + ( B y - A y ) + ( Bz - A z ) ù
2 2 2
ëê ûú
________________________________________________________________________
r r
Nonzero vectors a and b are added according to the parallelogram rule, as shown in
Fig. 2.1.3. The parallelogram used in this construction is formed in the plane that contains the
r r
intersecting vectors a and b . The vector sum is written as
r r r
c=a+b (2.1.1)
Figure 2.1.3 Addition of Vectors

Addition of vectors and multiplication of vectors by scalars obey the following rules
(Kreyszig, 2011)
r r r r
a+b = b+a
r r r (2.1.2)
( α + β ) a = αa + βa
where α and β are scalars.
r r
Using the definition of vector addition, the plane formed by intersecting vectors a and b
r
in Fig. 2.1.3 may be called the x ¢-y¢ plane, with the x ¢ axis along vector a and the y¢ axis
perpendicular to the x ¢ axis, as shown in Fig. 2.1.4. The x ¢-y¢ frame is called right handed if
rotating the x ¢ axis to the y¢ axis with the right hand leads to the thumb pointing out of the plane
r r
toward the viewer. Plane trigonometry can now be used to calculate the length of vector a + b
rr r r r
( ) r
and the angle θ a,a + b between a and a + b ,
r r é rr 2 r r 2 ù1/ 2
ë
( ( )) ( ( ))
a + b = ê a + bcosθ a,b + bsinθ a,b ú
û (2.1.3)
rr r r rr
( ) ë
r
( ) ( ( ))
θ a,a + b = Arctan é bsinθ a,b / a + bcosθ a,b ù
û
r r
These calculations define the vector a + b , but are not convenient for computation.
20
Figure 2.1.4 Sum of Vectors in x ¢-y¢ Plane of Intersection
Orthogonal reference frames are used extensively in representing vectors. Use in this text
is limited to right-hand orthogonal reference frames; i.e., with mutually orthogonal x, y, and z
axes that are ordered by the finger structure of the right hand, as shown in Fig. 2.1.5. Such a
frame is called a Cartesian reference frame.
z
Figure 2.1.5 Right-Hand Orthogonal Reference Frame

r
A vector a can be resolved into components a x , a y , and a z along the x, y, and z axes of
a Cartesian reference frame, via orthogonal projection onto the axes, as shown in Fig. 2.1.6.
These components are called the Cartesian components of a vector. The unit coordinate vectors
r r r
i , j , and k are unit vectors that are directed along the positive x, y, and z axes, respectively, as
shown in Fig. 2.1.6. In vector notation,
r r r r
a = a x i+a y j+a z k (2.1.4)
Figure 2.1.6 Components of a Vector
21
r rr
r
Denote the angle from vector a to vector b in the plane that contains them by θ a,b , ( )
with counterclockwise as positive about the normal to the plane of the vectors that points toward
r
the viewer, as shown in Fig. 2.1.3. In terms of angles between the vector a and unit vectors
along the positive x, y, and z axes, the Cartesian components of a vector, or simply components
r
of a vector a are
rr
a x = acosθ i,a ( )
rr
a y = acosθ j,a ( ) (2.1.5)
rr
a z = acosθ k,a ( )
rr rr rr
The quantities cosθ ( i,a ) , cosθ ( j,a ) , and cosθ ( k,a ) are called the direction cosines of vector a .
r
Note that if the viewer had been on the back side of the plane of Fig. 2.1.3, the counterclockwise
r rr
r
( )
angle from a to b would be 2π - θ a,b . However, cos ( 2π - θ ) = cosθ , so the viewpoint does
not influence the value of direction cosines.
r r
Addition of vectors a and b may be expressed in terms of their components, using Eq.
(2.1.4), as
r r r r r r
c = a + b = ( a x + b x ) i + ( a y + b y ) j + ( a z + bz ) k
r r r (2.1.6)
º cx i + c y j + cz k
r
where c x = a x + bx , c y = a y + b y , and c z = a z + bz are the components of vector c . Thus,
r r r
addition of vectors occurs component-by-component. Using this idea, three vectors a , b , and c
may be added to show that
r r r r r r
( ) (
a+b +c = a+ b+c ) (2.1.7)
r r
The scalar product (sometimes called the dot product) of vectors a and b is defined as
the product of their magnitudes and the cosine of the angle between them; i.e.,
r r rr
a × b = abcosθ a,b ( ) (2.1.8)
This definition is purely geometric, so it is independent of any reference frame in which the
vectors may be represented.
r r
Note that if two vectors a and b are nonzero; i.e., a ¹ 0 ¹ b , then their scalar product is
rr
( )
zero if and only if cosθ a,b = 0 . Two nonzero vectors are said to be orthogonal vectors if their
scalar product is zero.
rr rr
( ) ( )
Since θ b,a = 2π - θ a,b and cos ( 2π - θ ) = cosθ , the order of terms appearing on the
right side of Eq. (2.1.8) is immaterial. Thus,
r r r r
a ×b = b×a (2.1.9)
22
The scalar product of two vectors may be interpreted as the product of the magnitude of one of
the vectors times the orthogonal projection of the other vector onto that vector. To see this, refer
r r r
r
to Fig. 2.1.4, where the projection of vector b onto a has length bcosθ a, b . ( )
Based on the definition of scalar product, the following identities hold for the unit
rr r
coordinate vectors i, j, and k :
r r r r r r
i × j = j×k = k × i = 0
r r r r r r (2.1.10)
i × i = j× j = k ×k =1
r
For any vector a ,
r r
a × a = aacos0 = a 2
While not obvious on geometrical grounds, the scalar product satisfies the relation (Kreyszig,
2011)
r r r r r r r
( )
a + b × c = a× c + b×c (2.1.11)
Using Eq. (2.1.11) and the identities of Eq. (2.1.10), a direct calculation yields
r r
a × b = a x bx + a y by + a z bz (2.1.12)
r r
The vector product (sometimes called the cross product) of vectors a and b is defined
as the vector
r r rr r
a ´ b º absinθ a,b u ( ) (2.1.13)
r
where u is the unit vector that is orthogonal (perpendicular) to the plane of intersection of
r r
vectors a and b , taken in the positive right-hand coordinate direction, as shown in Fig. 2.1.7. If
r
the viewer were behind the plane of Fig. 2.1.7, the unit normal to the plane would be - u and the
r rr
r
( )
counterclockwise angle from a to b would be 2π - θ a,b . Then, the vector product would be
r r rr r
( ( ) ) ( - ur ) = absinθ ( a,b
a ´ b º absin 2π - θ a,b
r r
)u
since sin ( 2π - θ ) = sinθ . This is the same result as in Eq. (2.1.13) , so the viewpoint does not
influence the value of the vector product. Since the definition of vector product is purely
geometrical, the result is independent of the reference frame.
Figure 2.1.7 Vector Product
23
r r r r r rr
r r
( )
It is important to note that if a ¹ 0 ¹ b and a ´ b = 0 , then sinθ a,b = 0 and the
r r r
vectors are parallel. Equivalently, if a ´ b = 0 , then a = a b for some scalar a .
r r
Since reversal of the order of vectors a and b in Eq. (2.1.13) yields the opposite
r
direction for the unit vector u ,
r r r r
a ´ b = -b ´ a (2.1.14)
Analogous to Eq. (2.1.11) , the vector product satisfies (Kreyszig, 2011)
r r r r r r r
( )
a + b ´ c = a ´ c + b´ c (2.1.15)
rr r
Since θ ( a,a ) = 0 for any vector a ,
r r r r
a ´ a = a 2sin0u = 0
From the definition of unit coordinate vectors and vector product, the following identities
are valid:
r r r r r r r
i ´ i = j´ j = k´ k = 0
r r r r r
i ´ j = - j´ i = k
r r r r r (2.1.16)
j ´ k = -k ´ j = i
r r r r r
k´ i = -i ´k = j
Using the identities of Eqs. (2.1.15) and (2.1.16), the vector product of two vectors may be
expanded and written in terms of their components as
r r r r r r
c = a ´ b = ( a ybz - a z b y ) i + ( a z b x - a x bz ) j + ( a x b y - a y bx ) k
r r r (2.1.17)
º cx i + cy j + cz k
where c x = a y b z - a z b y , c y = a z bx - a x bz , and c z = a x by - a y bx .
2.1.2 Algebraic Vectors

r
Recall from Eq. (2.1.4) that a vector a can be written in component form in a Cartesian
x-y-z frame as
r r r r
a = a x i+a y j+a z k
r
The geometric vector a is thus uniquely defined by its Cartesian components, which may be
written in matrix notation as
éa x ù
ê ú T
a = ê a y ú = éë a x ay a z ùû (2.1.18)
êa ú
ë zû
This is the algebraic representation of a geometric vector. Note that the algebraic representation
rr r
of vectors is dependent on the Cartesian reference frame selected; i.e., vectors i, j, and k . Some
24
of the purely geometric properties of vectors are thus lost, and properties of the reference frame
that is used in defining components of vectors come into play. This apparent problem will in fact
become a valuable tool in spatial kinematics and dynamics.
An algebraic vector is defined as a column matrix, or column vector. When an algebraic
vector represents a geometric vector in three-dimensional space, it has three components.
Algebraic vectors with more than three components will also be employed in kinematic and
dynamic analysis of multibody systems. If a = [a1 a 2 L a n ]T , the algebraic vector a is
called an n-vector that belongs to n-dimensional Euclidean space, denoted R n .
r r
If two geometric vectors a and b are represented in algebraic form as
T
a = éëa x ay a z ùû
T
(2.1.19)
b = éë b x by b z ùû
then Eq. (2.1.6) shows that their vector sum is represented in algebraic form as
T
c = a + b = éë a x + b x a y + by a z + b z ùû (2.1.20)
This simple algebraic relation, which is ideal for computation, is in contrast with the
trigonometric complexity of the parallelogram rule of vector addition of Fig. 2.1.3.
___________________________________________________________________
Example 2.1.2: The algebraic representation of vectors
r r r r
a = i + 2j + 3k
r r r r
b = -i + j - k
is
a = [1 2 3]
T
b = [ -1 1 -1]
T
r r r r r r r r r r r r
The algebraic representation of the sum c = a + b = i + 2j + 3k - i + j - k = 0 i + 3 j + 2k is
c = a + b = [ 0 3 2]
T
________________________________________________________________________
r r
Geometric vectors a and b are equal vectors if and only if their Cartesian components are
r
equal; i.e., a = b . Multiplication of a vector a by a scalar α occurs component-by-component, so
the geometric vector
r r r r r r r
αa = α(a x i + a y j + a z k) = αa x i + αa y j + αa z k
is represented by the algebraic vector αa .
25
Since there is a one-to-one correspondence between geometric vectors and 3 ´ 1 algebraic
vectors that are formed from their components in a specified Cartesian reference frame, no
distinction other than notation will be made between them in the remainder of the text.
r r
The scalar product of geometric vectors a and b may be expressed in algebraic form,
using the result of Eq.(2.1.12) , as
r r
a × b = a x b x + a y b y + a z b z = a Tb (2.1.21)
This simple algebraic relation, which is ideally suited for digital computation, contrasts with the
calculation of Eq. (2.1.3).
______________________________________________________________________
r r
Example 2.1.3: The scalar product of vectors a and b , or a and b , in Example 2.1.2 is
é - 1ù
r r
a × b = a b = [1 2 3] êê 1 úú = -2
T
êë - 1úû
From the definition of scalar product,
r r rr
aT b = a × b = abcosθ a,b ( )
rr
( ) ( ) ( )
1/ 2 1/ 2
and, with a = aT a = 14 and b = b T b = 3 , cosθ a,b = -2 / 42 . Thus,
rr rr
( )
θ a,b = 1.88 or 4.40 rad. Additional information is required to uniquely determine θ a,b . ( )
_______________________________________________________________
A 3 ´ 3 skew-symmetric matrix a% ; i.e., a% = -a% T , associated with a 3 ´ 1 algebraic vector
T
a = éë a x , a y , a z ùû is defined as
é 0 -a z ay ù
ê ú
a% º ê a z 0 -a x ú (2.1.22)
ê -a y ax 0 úû
ë
Note that the overhead symbol %g (pronounced tilde) indicates that the components of the
associated vector are used to generate a skew-symmetric 3 ´ 3 matrix. Conversely, any 3 ´ 3
skew-symmetric matrix B of the form
é 0 b12 b13 ù
ê ú
B º ê - b12 0 b 23 ú
êë - b13 - b 23 0 úû
can be written as B = b% , where b = [ - b 23 b13 - b12 ]T ; i.e.,
26
é 0 b12 b13 ù é 0 - b z b y ù æ²é - b 23 ù ö
ê ú ê ú % çê ú÷
B = ê - b12 0 b23 ú = ê b z 0 - b x ú º b = ç ê b13 ú ÷ (2.1.23)
ê 0 úû ç ê -b ú ÷
ëê - b13 - b 23 0 ûú ë - b y b x è ë 12 û ø
r r r
The vector product c = a ´ b , which is expanded in component form in Eq. (2.1.17), can
be written in algebraic vector form as
é a y bz - a z by ù
ê ú
c = ab
% = ê a z b x - a x bz ú (2.1.24)
êa b - a b ú
ë x y y xû
This result is the reason the %g operator is introduced. It gives a computationally practical means
of evaluating the vector product of two vectors that are represented in algebraic form. The
practicality is clear by comparing Eqs. (2.1.13) and (2.1.24).
________________________________________________________________________
r r r
Example 2.1.4: The algebraic representation of the vector product c = a ´ b in Example 2.1.2 is
é 0 -3 2 ù é - 1ù é - 5ù
% = ê 3 0 -1ú ê 1 ú = ê -2 ú
c = ab ê úê ú ê ú
êë -2 1 0 úû êë - 1úû êë 3 úû
________________________________________________________________________
2.1.3 Vector Identities
It is helpful to develop identities involving the %g operation. First, note that
é 0 az -a y ù
ê ú
a% T = ê - a z 0 a x ú = -a% (2.1.25)
ê ay -a x 0 úû
ë
Also, for a scalar α ,
é 0 - αa z αa y ù
ê ú
αa% = ê αa z 0 - αa x ú = (²
αa ) (2.1.26)
ê - αa y αa x 0 úû
ë
For any algebraic vectors a and b, a direct calculation shows that
% = -ba
ab % (2.1.27)
r r r r
which agrees with the vector product relation a ´ b = - b ´ a of Eq. (2.1.14) . Direct calculation
shows that
% =0
aa (2.1.28)
r r
which agrees with a ´ a = 0 . By Eq. (2.1.25),
27
( aa
% )
T
= aT a% T = -aT a% = 0 (2.1.29)
Direct calculation shows that

% % = ba T - aT bI
ab (2.1.30)
where I is the 3 ´ 3 identity matrix. Direct calculation also shows that
%± = ba T - abT
ab (2.1.31)
± % % - ba
% = ab
ab %% (2.1.32)
From Eqs. (2.1.31) and (2.1.32),
% % + abT = ba
ab % % + baT (2.1.33)
Finally, direct calculation shows that
(²
a + b ) = a% + b% (2.1.34)
Matrix implementation of vector operations permits the systematic organization of

calculations, which is essential for computer application.
________________________________________________________________________
Example 2.1.5: To define the position and orientation of a Cartesian x ¢ - y¢ - z¢ reference frame
with its origin at point P in an x-y-z Cartesian reference frame, it is first essential to locate point
P with vector r P , as shown in Fig. 2.1.8. If a second point Q that is not coincident with P is
located on the x ¢ axis by vector r Q , the unit vector f along the x ¢ axis is
f=
1
r -r
Q P (
rQ - rP )
If a third point R that is not coincident with P and not on the x ¢ axis is located in the x ¢-y¢ plane
by vector r R , then the vector product of f and r R - r P ¹ 0 is a vector in the direction of the z¢
axis; i.e.,
h=
1
(
f% r R - r P )
(
%f r R - r P )
Point R could, but need not, be on the y¢ axis. Finally, the unit vector g along the y¢ axis is
%
g = hf
28
Figure 2.1.8 Points Defining a Cartesian x ¢-y¢-z¢ Reference Frame
Thus, three distinct noncollinear points P, Q, and R in the x ¢-y¢ plane define unit vectors
f, g, and h along the x ¢ , y¢ , and z¢ axes, hence defining the position and orientation of the
x ¢-y¢-z¢ Cartesian reference frame
_______________________________________________________________________
2.1.4 Time Derivatives of Vectors
In analyzing velocities and accelerations, the time derivatives of vectors that locate points
r
must be calculated. Consider a time-dependent vector a ( t ) with components
a ( t ) = éë a x (t) a y (t) a z (t) ùû in a stationary Cartesian reference frame; i.e., it does not depend
T
r r r r
on time and i , j , and k are constant. The time derivative of a vector a ( t ) is the velocity of the
point that is located by the vector in a stationary x-y-z frame,
r r r r r r r r r
r d
a& º
dt
( )
a x ( t ) i+a y ( t ) j+a z ( t ) k = d a x ( t ) i + d a y ( t ) j + d a z ( t ) k = a& x i + a& y j + a& z k
dt dt dt
r r r
Note that this is only valid if i , j , and k are not time dependent. In algebraic vector notation,
this is
a& º d a ( t ) = éë a& x
T
a& y a& z ùû (2.1.35)
dt
Thus, for a vector that is represented by its components relative to a stationary Cartesian
reference frame, its derivative is obtained by differentiating its components.
29
_______________________________________________________________________
Example 2.1.6: A particle P moves with constant speed along a circle of radius R in the x-y
plane of a stationary Cartesian reference frame, as shown in Fig. 2.1.9. Its position vector is thus
é Rcosωt ù
r ( t ) = êê Rsinωt úú
êë 0 úû
where R and ω are constant. Its velocity is thus

é - Rωsinωt ù
r& ( t ) = êê Rωcosωt úú
êë 0 úû
Its speed is the magnitude of its velocity; i.e., r& (t) = (Rw) 2 (sin 2 wt + cos 2 wt) = Rw . Since
r Tr& = - R 2wcosωtsinωt + R 2wcosωtsinωt = 0

the vectors r and r& are orthogonal and r& is tangent to the circle
Figure 2.1.9 Particle Moving on Circular Path with Constant Speed

______________________________________________________________________________________
The derivative of the sum of two algebraic vectors is
d
dt
( a ( t ) + b ( t )) = a& + b& (2.1.36)
which is the differentiation rule of calculus that the derivative of a sum is the sum of the
derivatives.
The following vector forms of the product rule of differentiation are also valid:
d ( αa ) = α& a + αa& (2.1.37)
dt
( )
d aT b = a& T b + a T b& = b T a& + aT b&
dt
(2.1.38)
d ( ab
% ) = ab
&% + ab
% & = ab
% & - ba
%& (2.1.39)
dt
where α ( t ) is a scalar function of time and a and b are time dependent vectors. Note also that
30
a%& = a%& (2.1.40)
If the length of a vector a ( t ) is constant; i.e. a ( t ) a ( t ) = c , where c is a constant, then

T
Eq. (2.1.38) yields a& T a + aT a& = 2a& T a = 0 , so

a& Ta = 0 (2.1.41)
If a is a position vector that locates a point, then a& is the velocity of that point. Hence, Eq.
(2.1.41) shows that the velocity of a point whose distance from the origin is constant is
orthogonal to the position vector of the point. Note that Eq. (2.1.41) is satisfied by the velocity
vector of Example 2.1.7, which is to be expected since r T r = R 2 .
The second time derivative of a ( t ) is the acceleration of the point that is located in the
stationary Cartesian x-y-z frame by the vector a(t) ;i.e.,
T
é d2 d2 d2 ù
a º ( a& ( t ) ) = ê 2 a x ( t )
d
&& a (t) 2 z ( )ú
a t (2.1.42)
dt ë dt dt 2 y dt û
Thus, for vectors that are written in terms of their components in a stationary Cartesian reference
frame, acceleration may be calculated in terms of the second time derivatives of the components
of the vector.
_______________________________________________________________________
Example 2.1.7: The acceleration of the particle located by the vector r ( t ) in Example 2.1.7 is
é - Rω2 cosωt ù é Rcosωt ù

ê ú
r ( t ) = ê - Rω2sinωt ú = - ω2 êê Rsinωt úú = - ω2r ( t )
&&
ê 0 ú êë 0 úû
ë û
This is the classical centripetal acceleration of a point that moves with constant velocity on a
circular path, since the direction of &r& is opposite to the direction of r ( t ) .
________________________________________________________________________
Since vector relations derived in this section are used often in kinematic modeling of
mechanical systems, the most common relations are summarized as “Key Formulas” at the end
of the section. Such summaries are presented at the end of sections in the text in which such
relations are derived.
31
Geometric vectors, with the operations of addition, scalar product, and vector product, have the
virtue of being independent of reference frames. Their downside is the difficulty in representing
vector operations and differentiation with respect to time in a form that is suitable for digital
computation.
Algebraic vectors, in contrast, suffer from being dependent on the Cartesian reference frame in
which they are represented. Their saving grace is that they are ideally suited for matrix
representation and digital computer implementation. Time derivatives of algebraic vectors are
also easily defined and operated on by the rules of calculus and implemented in digital
computation.
Key Formulas
é 0 -a z ay ù
ê ú
a% º ê a z 0 -a x ú (2.1.22)
ê-a ax 0 úû
ë y
é 0 b12 b13 ù é 0 - bz by ù æ²é - b 23 ù ö
ê ú ê ú % çê ú÷
B = ê - b12 0 b23 ú = ê b z 0 - b x ú º b = ç ê b13 ú ÷ (2.1.23)
0 ûú êë - b y 0 úû ç ê -b ú ÷
ëê - b13 - b 23 bx è ë 12 û ø
a% T = -a% ab %
% = -ba % =0
aa (2.1.25) (2.1.27) (2.1.28)
% % = ba T - aT bI
ab %± = ab
ab % % - ba
%% (2.1.30) (2.1.32)
% % + abT = ba
ab % % + baT (2.1.33)
32
2.2 Matrix Algebra and Multivariable Calculus
Matrix algebra and multivariable calculus are the underpinning of computational
kinematics and dynamics. Definitions and basic results in matrix and linear algebra are reviewed,
in light of their pervasive use in the text, including the Lagrange multiplier theorem that is
critically important in mechanical system dynamics. Similarly, basic concepts, notation, and
results of multivariable calculus that are used throughout the text are presented. Continuity of
functions is defined using metric properties of the Euclidean vector space and extended to
representation in terms of open sets that provide the foundation for Euclidean space as a
topological vector space. Important properties of the space, in which kinematics and dynamics
take place are summarized for future use. The implicit function theorem that is indispensable in
determining when there are solutions of nonlinear equations is presented and applied in
constructing such solutions. Finally, the iterative Newton-Raphson method that is the cornerstone
for numerical solution of nonlinear equations of kinematics and dynamics is presented.
2.2.1 Matrix Algebra
Matrix algebra permits systematic representation of linear equations. Matrix manipulation
also allows for organized development, simplification, and solution of systems of linear
equations. A matrix is defined as a rectangular array of real numbers. If a matrix has m rows and
n columns, its matrix dimension is m×n . A matrix is denoted by a boldface capital letter if m and
n are greater than one, and is written in the form
é a 11 a 12 L a1n ù
êa a 22 L a 2n ú
A= ê 21 ú (2.2.1)
ê M M M M ú
ê ú
ë a m1 a m2 L a mn û
where the scalar element a ij is located in the i-th row and j-th column. The transpose of a matrix
is formed by interchanging rows and columns and is designated by the superscript. g T . If a ij is the
i-j element of matrix A, a ji is the i-j element of its transpose, denoted AT .
A matrix with only one column is called a column matrix, or column vector, as in Section
2.1, and is denoted by a boldface lowercase letter; i.e., a. A matrix with only one row is called a
row matrix, or row vector, and is denoted by a boldface lowercase letter. An m×n matrix can be
T
considered as being constructed of n column matrices a j = ëé a1j L a mj ùû , j = 1, L , n , or m
row matrices b i = [ a i1 L a in ] , i = 1, L , m ; i.e.,
é b1 ù
A = [ a1 L a n ] = ê M ú (2.2.2)
ê ú
êë b m úû
______________________________________________________________________________
Example 2.2.1: The 2×3 matrix
33
é1 2 0 ù
A=ê ú
ë 2 3 1û
may be thought of as made up of three 2×1 column matrices,
a1 = [1 2 ] , a 2 = [ 2 3] , a 3 = [ 0 1]
T T T
or two 1×3 row matrices,

b1 = [1 2 0] , b 2 = [ 2 3 1]
______________________________________________________________________________
A square matrix has an equal number of rows and columns. A diagonal matrix is a
square matrix with a ij = 0 for i ¹ j , and at least one nonzero diagonal element. An n×n diagonal
matrix A can be represented as
A = diag ( a11 a 22 L a nn ) (2.2.3)
The n×n identity matrix, denoted I, or I n , is a diagonal matrix with unit values on the diagonal.
A zero matrix of any dimension, designated by a bold 0, is a matrix with all elements equal to
zero.
If matrices A and B have the same dimension, they are defined to be equal matrices if
a ij = bij , for all i and j; i.e., all elements in the same positions are equal. The sum of matrices A
and B that have the same dimension is a matrix with the same dimension, defined as
C= A+B (2.2.4)
where cij = a ij + bij , for all i and j; i.e., matrices with the same dimension add element by
element. The difference between matrices A and B of the same dimension is defined as
D= A-B
where d ij = a ij - b ij , for all i and j. If matrices A, B, and C have the same dimension,
( A + B) + C = A + ( B + C) = A + B + C (2.2.5)
Similarly, for matrices A and B with the same dimension,

A+B = B+ A (2.2.6)
________________________________________________________________________
Example 2.2.2: The sum of the 2×2 matrices
é 1 1ù é0 2ù
A=ê ú , B=ê ú
ë 2 1û ë1 1 û
is
é1 3 ù
C= A+B = ê ú
ë3 2 û
34
and the difference is
é1 -1ù
D = A-B = ê ú
ë1 0 û
_______________________________________________________________________
Let A be an m×p matrix and B be a p×n matrix, written in the form
é d1 ù
A º éë a ij ùû = ê M ú , B º éë bij ùû = [ b1 L b n ] (2.2.7)
ê ú
ëêd m ûú
The matrix product of A and B is defined as the m×n matrix
C = AB (2.2.8)
where
p
cij = å a ik b kj (2.2.9)
k =1
or, in terms of the rows of A and columns of B,

cij = di b j (2.2.10)
________________________________________________________________________
Example 2.2.3: The product of the 2×2 matrix B of Example 2.2.2 and the 2×3 matrix A of
Example 2.2.1 is the 2×3 matrix
é 0 2 ù é1 2 0 ù é 4 6 2 ù
C = BA = ê úê ú=ê ú
ë 1 1 û ë 2 3 1û ë 3 5 1 û
________________________________________________________________________
It is important to note that the product of two matrices is defined only if the number of
columns in the first matrix equals the number of rows in the second matrix. Even if matrix
multiplications AB and BA are defined; i.e., the matrices are square and of the same dimension,
in general,
AB ¹ BA (2.2.11)
To see that this is the case, the matrices of Example 2.2.2 yield
é 1 1ù é 0 2 ù é1 3ù é 4 2 ù é 0 2 ù é 1 1ù
AB = ê úê ú=ê ú¹ê ú=ê úê ú = BA
ë 2 1û ë 1 1 û ë1 5û ë 3 2 û ë 1 1 û ë 2 1û
If A and B are m×p matrices and C is a p×n matrix, direct manipulation verifies that
( A + B ) C = AC + BC (2.2.12)
Similarly, if A is an m×p matrix, B is a p×q matrix, and C is a q×n matrix,
35
( AB ) C = A ( BC) = ABC (2.2.13)
Multiplication of a matrix A by a scalar α is defined as

C = αA (2.2.14)
where cij = αa ij , for all i and j; i.e., all elements in the matrix are multiplied by the scalar.
If a ij = a ji , for all i and j, the square matrix A is called a symmetric matrix; i.e., A = A T .
If a ij = -a ji , for all i and j, the square matrix A is called a skew-symmetric matrix; i.e., A = - A T .
Note that for skew-symmetric matrices, a ii = 0 , for all i.
The transpose of the sum of two matrices is the sum of their transposes; i.e.,
( A + B) = AT + BT
T
(2.2.15)
Also, if A is an m×p matrix and B is a p×n matrix, then
( AB ) = BT A T
T
(2.2.16)
A set of n ´ 1 column matrices a j , j = 1,L , m , is called linearly dependent if there are

constants α j , j = 1, L m , that are not all zero such that
m
åα a
j=1
j j =0 (2.2.17)
If a set of column matrices is not linearly dependent, it is called linearly independent.

Equivalently, column matrices a j , j = 1, L , m , are linearly independent if and only if Eq.
(2.2.17) implies that α j = 0, j = 1, L, m.
______________________________________________________________________
Example 2.2.4: To see if the column matrices
é1 ù é1ù é1ù
b1 = ê 1 ú , b 2 = ê 0ú , b3 = êê 2 úú
ê ú ê ú
êë 0 úû êë 1 úû êë -1úû
m
are linearly independent, form åβ b
j=1
j j = 0 . This may be written as a matrix equation in the β i ,
é β1 ù é 1 1 1 ù é β1 ù
[ b1 b2 b 3 ] êêβ 2 úú = êê 1 0 2 úú êêβ 2 úú = 0
êë β 3 úû êë 0 1 -1úû êëβ 3 úû
For any β 3 , β1 = -2β3 and β 2 = β 3 satisfy these equations and the b j are linearly dependent.
________________________________________________________________________
36
Consider the n ´ m matrix A = [ a1 L a m ] with columns a1 ,...., a m . If a linear
combination of the columns of A is zero; i.e.,
m
Aa = å α ja j = 0 (2.2.18)
j=1
for some a = [ α1 L α m ] ¹ 0 , then the columns of A are linearly dependent. Otherwise, they
T
é d1 ù
are linearly independent. Rows d1 ,..., d n of D = êê M úú are linearly dependent if
êë d n úû
n
b T D = å β id i = 0 (2.2.19)
i =1
for some b = [β1 L β n ] ¹ 0 . Otherwise they are linearly independent.

T
The row rank (column rank) of a matrix is defined as the largest number of linearly
independent rows (columns) in the matrix. The row and column ranks of any matrix are equal
(Kreyszig, 2011), hence defining the rank of the matrix. The rank of a matrix is also equal to the
dimension of the largest square submatrix that is obtained by deleting rows and columns with
nonzero determinant (Kreyszig, 2011). A square matrix with linearly independent rows or
columns is said to have full rank. Thus, a square matrix has full rank if and only as its
determinant is not zero.
When a square matrix does not have full rank; i.e., A = 0 , it is called a singular matrix.
For a singular matrix with columns a i , i = 1L n , there are scalars u i , i = 1,L, n, that are not all 0
n
åa u = 0 . With u = [ u1 ... u n ] , this is the matrix equation

T
such that i i
i =1
Au = 0 (2.2.20)
This shows that a square matrix A is singular if and only if there is a nonzero column vector u
such that Eq. (2.2.20) holds.
A square matrix A of full rank is called a nonsingular matrix. For such a matrix, there is
an inverse matrix (Kreyszig, 2011), denoted A -1 , such that
AA -1 = A -1A = I (2.2.21)
Using the definition of Eq. (2.2.21) and Eq. (2.2.16), (Kreyszig, 2011),
(A ) = (A )
-1 T T -1
(2.2.22)
and if matrices A and B of the same dimension are nonsingular,
( AB )
-1
= B - 1A - 1 (2.2.23)
A special nonsingular matrix that arises often in kinematics is called an orthogonal

matrix A, with the property that
37
A T A = AA T = I (2.2.24)
That is, from the definition of Eq. (2.2.21),
A -1 = A T (2.2.25)
Since constructing the inverse of a nonsingular matrix is time consuming, it is important
to know when a matrix is orthogonal. In case a matrix is orthogonal, its inverse is easily
constructed using Eq. (2.2.25), which requires no arithmetic operations.
________________________________________________________________________
Example 2.2.5: The 2×2 matrix
é cosθ -sinθ ù
A=ê ú
ë sinθ cosθ û
is orthogonal, for any value of θ , since
é cos2θ + sin 2θ 0 ù
AT A = ê 2 ú
=I
ë 0 cos θ + sin θ û
2
________________________________________________________________________
2.2.2 Lagrange Multiplier Theorem of Linear Algebra
A result that plays an important role in constrained multibody dynamics is the following
theorem of linear algebra.
Lagrange Multiplier Theorem: Let c be an n-vector of constants, x be an n-vector of
variables, and B be an m ´ n constant matrix. If
x Tc = 0 (2.2.26)
for all x that satisfy
Bx = 0 (2.2.27)
then there exists an m-vector λ of Lagrange multipliers such that
c T x + l T Bx = 0 (2.2.28)
for arbitrary x. Thus, c T + l T B = 0 , or
c + B Tl = 0 (2.2.29)
Further, if B has full row rank, λ is unique.
Since this theorem is not found in the literature on mechanical system kinematics and
dynamics, a short proof is presented here. To prove the theorem, define the null space of B as
N(B )= {x Î R n : Bx = 0} and the range of BT as R(B T ) = {y Î R n : y = B T a, for some a Î R m } .
The fundamental theorem of algebra (Strang, 1980) implies
R(B T ) = ( N(B) ) º {y Î R n : x T y = 0 for all x Î N(B )} . The statement that Eq. (2.2.26) holds for
^
all x that satisfy Eq.(2.2.27) is equivalent to c Î N(B ) ^ = R( B T ) . Thus, there exists an a Î R m

such that c = B T a . Setting l = -a yields the result of Eq. (2.2.29). The result of Eq. (2.2.28)
38
follows. To see that λ is unique if B has full row rank, assume there is a λ ¹ λ that satisfies Eq.
(2.2.29). Then, BT ( λ - λ ) = 0 and, since BT has full column rank, λ - λ = 0 and λ is unique.
This completes the proof.
2.2.3 Multivariable Calculus
Just as in differentiation of a vector whose components are functions of t, the derivative
of a matrix whose elements depend on t may be defined. Consider a matrix B ( t ) = éë b ij ( t ) ùû . The
derivative of B(t) is defined as
& = d B = é dbij ù = é b& ù

B ê ú ë ij û (2.2.30)
dt ë dt û
With this definition and elementary rules of differentiation,
d
( B + C ) = B& + C& (2.2.31)
dt
d
( BC ) = BC &
& + BC (2.2.32)
dt
d
( αB ) = α& B + αB& (2.2.33)
dt
where α = α ( t ) is a scalar function of t.
In dealing with systems of nonlinear equations in several variables, which govern the
kinematics and dynamics of mechanical systems, it is essential that multivariable calculus be
employed. A basic construct in multivariable calculus is definition of real Euclidean space
{ }
R n º q = [q1 L q n ] : q i is real, i = 1,L, n , and the scalar product of u, v Î R n is defined as
T
n
u, v º u T v = å u i v i . To introduce notation used here, let q Î R n , a(q) be a scalar differentiable
i =1
function of q, and f ( q ) = éëf1 ( q ) L f m ( q ) ùû be an m-vector of scalar differentiable functions

T
of q. Using i as row index and j as column index, the following matrix calculus notations are
defined:
¶a é ¶a ù
aq º =ê ú (2.2.34)
¶q ëê ¶q j ûú
1×n
¶f é ¶f i ù
fq º =ê ú (2.2.35)
¶q êë ¶q j úû m×n
Note that the derivative of a scalar function with respect to a vector variable in Eq.
(2.2.34) is a row matrix. This is one of the few matrix symbols in the text that is a row matrix,
rather than the more common column matrix. Note also that, as defined by Eq. (2.2.35), the
derivative of a column vector function, whose elements are functions of a vector variable, is a
matrix. The subscript notation used here to denote differentiation is helpful in deriving needed
39
relations, without becoming entangled in cumbersome index and partial differentiation notation.
To take advantage of this notation, however, it is critically important that correct matrix
definitions of derivatives be used.
The partial derivative of the scalar product of m-vector functions
g ( q ) = éë g1 ( q ) L g m ( q ) ùû and h ( q ) = éë h 1 ( q ) L h m ( q ) ùû of an n-vector variable q, by
T T
careful manipulation, yields the product rule of differentiation
¶ T ¶ æ m ö é ¶ æ m öù
( g Th ) q = ¶q
( g h ) = ç å
¶q è k =1
g k k÷ = ê
h ç å gk h k ÷ ú
ø êë ¶q j è k =1 ø úû
é m æ ¶g ¶h ö ù é æ m ¶g ö ù é æ m ¶h öù
= ê å ç k h k + gk k ÷ ú = êç å h k k ÷ ú + êç å gk k ÷÷ ú (2.2.36)
ç ÷ ç ÷ ç
êë k =1 è ¶q j ¶q j ø úû êë è k =1 ¶q j ø úû êë è k =1 ¶q j ø úû
= h Tg q + g T h q
Note that what might have intuitively appeared to be the appropriate product rule of
( ) ¹ (g )
T
differentiation is not even defined, much less valid; i.e., g T h q h + g T hq .
q
If f ( g ) = éë f1 ( g ) L fm ( g ) ùû and g ( q ) = éë g1 ( q ) L g k ( q ) ùû , where q is an n-
T T
vector, the chain rule of differentiation is

é ¶f i ( g ( q ) ) ù é k æ ¶f ¶g ö ù é ¶f ù é ¶g ù
f (g(q ))q = ê ú = ê å çç i l
÷÷ ú = ê i ú ê i ú = fg g q (2.2.37)
êë ¶q j úû m×n êë l =1 è ¶g l ¶q j ø ûú m×n êë ¶g j úû m×k êë ¶q j úû k×n
If B is a constant m×n matrix and p and q are m-and n-vectors of variables, respectively,
the following useful relations may be verified:
¶
( Bq ) = ( Bq )q = B (2.2.38)
¶q
¶
¶p
( p T Bq ) = ( p T Bq ) = ( q T B T p ) = q T B T
p p
(2.2.39)
d T
dt
(p Bq ) = pT Bq& + qT BTp& (2.2.40)
2.2.4 Euclidean Space, Continuity of Functions, and Topologies

To deal with continuity of functions of several variables and their derivatives, the concept
of distance between vector variables is required. First, the n-dimensional Euclidean space is
{
R n = q = [q1 L q n ] : q i real, i = 1,L, n
T
} (2.2.41)
Elements of R n are called generalized coordinate vectors, or simply vectors, that obey the
following operations of addition and multiplication by a scalar:
40
r + s = [ r1 + s1 L rn + s n ]
T
(2.2.42)
aq = [ aq1 L aq n ]
T
With these operations, R n is a vector space.

The scalar product of vectors r and s in Euclidean space R n is defined as
n
r, s º r T s = å ri si (2.2.43)
1=1
and the distance between vectors r and s in R n is defined as

n
d (r , s ) º r - s, r - s = (r - s )T (r - s ) = å (r - s )
i
i i
2
= r -s n
(2.2.44)
where the quantity

n
qnº q, q = åq
i
2
i (2.2.45)
is the norm of q Î R n , with the properties

q n
³0
r +s n £ r n + s n
(2.2.46)
q n
= 0 if and only if q = 0
for all q, r, and s in R n . When the dimension n of R n is understood, the subscript n of q n

will
be suppressed. Endowed with this distance function, or norm, Euclidean space R n is a metric
space (Mendleson, 1962).
A function f (q ) = [f1 (q ) L f m (q )] Î R m of q Î R n , denoted f : R n ® R m is defined
T
to be continuous at q 0 Î R n if, for every e > 0 , there exists a d > 0 such that f (q) - f (q 0 ) <e,
m
for all q Î R n such that q - q0 < d . The set of functions f (q ) that are continuous at all q in a
n
subset W of R n , denoted W Ì R n , is called C0 (W ) . The set of all functions f (q ) for which all
partial derivatives of order up to and including k of each component f i (q ) are continuous at all q
in W is called C k (W ) .
With the norm of Eq. (2.2.45) and associated distance function of Eq. (2.2.44), the
operations of addition and multiplication by a scalar of Eq. (2.2.42) are continuous functions.
Euclidean space R n is thus a normed vector space with a scalar product.
While the foregoing definition of continuity and differentiability on a set W Ì R n are
valid, to obtain analytical results that are needed in kinematics and dynamics, attention must
often be limited to sets W with additional properties. In particular, for a point q 0 Î R n , the set
41
{ }
B e (q 0 ) = q Î R n : q - q 0 < e (2.2.47)
is called an open ball of radius e centered at q 0 . A set W Ì R n is defined to be an open set in R n

if, for each q Î W , there is an d > 0 such that Bd (q) Ì W . A set N is called a neighborhood of q
if it is open and q Î N . A set W Ì R n is defined as being closed if its compliment, denoted
C(W) º {q Î R n : q Ï W} , is open.
Open sets in R n satisfy the following properties (Mendelson, 1962):

(1) the empty set j is open,
(2) R n is open,
(3) if Oi , i = 1,L, k, with k finite, are open, their intersection
Ç Oi = {q Î R n : q Î Qi , 1 = 1,L, k} is open, and
k
i =1
(4) if Oa , aÎI, I a possibly infinite index set, are open, their union
È O a = {q Î R n : q Î Q a , for some a Î I} is open
aÎI
With these properties and the set t of all open sets in R n , the pair (R n , t) is defined to be a
topological space. Since R n is a vector space, it is in fact a topological vector space (Horvath,
1966). For any subset X of R n with open sets in X defined as t = {W Ç X : W Ì R n open} ,
(X, t ) is a topological subspace of R n with the induced topology (Mendelson, 1962). Since a
subset X of R n may not be closed under the operations of addition and multiplication by a
scalar, it may not be a vector space. For example, the set X = {q Î R 2 : q12 + q 22 = 1} , the unit
circle in R 2 , is not a vector space since for q Î X , 2q is not in X.
Topological subspaces (X, t ) of R n are Hausdorff; i.e., for any a, b Î X , a ¹ b , there
exist open sets Oa and Ob in t with a Î Oa , b Î Ob , and Oa Ç Ob = j (Mendelson, 1962). With
the foregoing structure, Euclidean space has further attractive properties. Since pairs of points in
the open ball of Eq. (2.2.47) can be connected by a straight line in the ball, Euclidean space is
locally path connected (Mendelson, 1962). Further, each open ball of radius ε contains a
{ }
bounded closed ball of radius ε / 2 , Be /2 (q 0 ) = q Î R n : q - q 0 £ e / 2 , Euclidean space is
locally compact (Mendelson, 1962).
An equivalent characterization of the foregoing e-d definition of continuity is that
f : R ® R m is continuous at q0 Î R n if, for every open set W Ì R m such that f (q0 ) ÎW , the
n
inverse image of W ; i.e., f -1 ( W ) º {q Î R n : f ( q) Î W} , is open (Mendelson, 1962). This result is

needed in defining sets of configurations that share important kinematic and kinetic properties.
2.2.5 Implicit and Inverse Function Theorems
A system of n equations in an n-vector u and an m-vector v of variables,
42
f ( u,v ) = éë f1 ( u,v ) L f n ( u,v ) ùû = 0
T
(2.2.48)
can, under certain circumstances, be solved for u as a function of v. Conditions that assure
existence, uniqueness, and smoothness of a solution are given by the implicit function theorem
(Corwin and Szczarba, 1982):
Implicit Function Theorem: Let the functions f i (u, v ) in Eq. (2.2.48) have k ³ 1
continuous derivatives with respect to vector variables u and v in a neighborhood of u 0
and v 0 and let Eq. (2.2.48) be satisfied by vectors u 0 and v 0 ; i.e., f ( u 0 ,v 0 ) = 0 . If the
é ¶f i (u0 ,v 0 ) ù
n×n sub-Jacobian fu ( u0 ,v 0 ) = ê ú is nonsingular, then there exists a unique
êë ¶u j úû
solution u of Eq. (2.2.48) that is a k-times continuously differentiable function of v; i.e.,
u = h( v) (2.2.49)
such that u0 = h ( v0 ) and
f ( h ( v ) ,v ) = 0 (2.2.50)
for all v in a neighborhood V 0 of v 0 .

It is important to be precise about what this theorem says and what it does not say. First,
while it assures existence of a smooth solution, given by Eq. (2.2.49), it does not prescribe a
specific formula for the solution. Second, it is critical that the sub-Jacobian fu ( u,v ) be evaluated
with vectors u 0 and v 0 for which f ( u 0 ,v 0 ) = 0 . If the sub-Jacobian is nonsingular at (u 0 , v0 )
but f ( u0 ,v 0 ) ¹ 0 , then the theorem says nothing; i.e., it does not apply. Failure to verify that
vectors u 0 and v 0 satisfy Eq. (2.2.48) is a common error in attempting to use the implicit
function theorem. Third, the theorem assures that the solution exists and is unique only in a
neighborhood V0 of v 0 , which may be a tiny set or it may be a set of large extent, and there may
or may not be solutions outside V0 . Finally, the existence of k continuous derivatives of the
solution is important in both the theory and numerical methods that are used in kinematics and
dynamics. In particular, iterative numerical solution methods for the equations of kinematics and
dynamics require existence of derivatives for use in computation. The implicit function theorem,
in many cases, assures that the needed derivatives exist.
______________________________________________________________________________
Example 2.2.6 With u Î R 2and v Î R1 , consider the equations
é u - u 22 ù
f (u, v ) = ê 1 ú=0 (2.2.51)
ë u 2 - u1 - v û
43
é 1 -2u 2 ù
The subJacobian with respect to u is fu (u, v) = ê
1 úû
which is nonsingular provided
ë -1
fu (u, v) = 1 - 2u 2 ¹ 0 . With u 0 = 0 and v 0 = 1 , fu (u0 , v 0 ) = 1 ¹ 0 . Does this assure existence of
continuous h(v) such that u = h(v) satisfies Eq. (2.2.51)? To check, substitute v = 1 into Eq.
(2.2.51), solve for u 2 = u1 + 1 , and obtain the quadratic equation u1 - (u1 + 1)2 = -u12 - u1 - 1 = 0 ,
which has solutions u1 = ( -1 ± 1 - 4) / 2 , which are not real. Thus, Eq. (2.2.51) has no real
valued solution for u as a function of v in a neighborhood of v 0 = 1 , appearing to contradict the
implicit function theorem. What is wrong?
The answer to the foregoing dilemma is provided by Fig. 2.2.1. Solutions of Eq. (2.2.51)
are intersections of the straight line and parabola. The dashed straight line with v = 1, does not
intersect the parabola, so there is no solution. The error in the foregoing analysis is
misapplication of the implicit function theorem. Since f ( 0,1) = [0 -1] ¹ 0 , the theorem says
T
nothing about solutions near (u0 , v0 ) = (0,1) .
Figure 2.2.1 Intersections of Straight Line and Parabola

If v 0 = -1, u 2 = u1 - 1 , so u1 - (u1 - 1)2 = -u12 + 3u1 - 1 = 0 , with solutions
T
u1 = 3 / 2 ± 5 / 4 . Selecting u0 = éë3 / 2 + 5 / 4 1 / 2 + 5 / 4 ùû , which is point A in Fig. 2.2.1,
f (u0 , v0 ) = 0 and the implicit function theorem applies, yielding the solution
T
u = h( v ) = (1 / 2) éë(1 - 2v) + 1 - 4v ( -1 - 2v) + 1 - 4v ùû in a neighborhood of v 0 = -1 . Note
that this is not a global solution of Eq. (2.2.51) in a neighborhood of v 0 = -1 . If
T
u0 = ëé3 / 2 - 5 / 4 1 / 2 - 5 / 4 ûù , which is point B in Fig. 2.2.1, implicit function theorem
T
criteria are satisfied and u = h( v ) = (1 / 2) éë(1 - 2v) - 1 - 4v ( -1 - 2v) - 1 - 4v ùû is a local
solution.
______________________________________________________________________________
A common situation in kinematic and dynamic analysis is a system of n equations in n
variables q Î R n ,
f (q ) = [f1 (q ) L f n (q )] = 0
T
(2.2.52)
44
Conditions under which Eq. (2.2.52) has a unique solution are provided by the inverse function
theorem (Corwin and Szczarba, 1982):
Inverse Function Theorem: Let U1 be an open set in R n and the function f of Eq.
(2.2.52) be in C1 ( U1 ) . If fq (q0 ) is nonsingular for some q0 Î U1 , then there is an open
set U Ì U1 with q 0 Î U such that the set V = f (U) = {w Î R n : w = f (q) for some q Î U}
is open and there is an inverse function f -1 Î C1 (V) such that f -1 ( f (q ) ) = q , for all
q Î V . Further, fq-1 (w ) = ( fq (q ) ) , where w = f (q) .
-1
The inverse function theorem requires that the regularity hypothesis holds on an open set
U1 , but only requires that the Jacobian matrix be nonsingular at a single point q0 Î U1 . It does
not require a-priori knowledge that Eq. (2.2.52) has a solution at even an isolated point in U1 , as
was required in the implicit function theorem. The result that V = f (U) is open is valuable in
applications.
2.2.6 Differential Constraints
Occasionally, constraints on motion of a system are specified in terms of velocities; e.g.,
in the form of a scalar first order differential equation
a T ( q, t ) q& + c ( q, t ) = 0 (2.2.53)
where a ( q, t ) and q are an n-vector of functions and an n-vector of variables, respectively.

Equivalently, Eq. (2.2.53) can be written as the differential constraint
a T ( q, t ) dq + c ( q, t ) dt = 0 (2.2.54)
whose left side is called a differential form in the variables q and t.
A natural question arises in consideration of an equation of the form of Eq. (2.2.54),
namely is it an exact differential form; i.e., is it the differential of some scalar function f ( q, t )
such that df º fq dq + f t dt = aT dq + cdt ? If it is, then Eq. (2.2.54) is equivalent to the algebraic
equation f ( q, t ) = constant .
To simplify the statement of conditions for exactness of differential forms, consider the
differential form
a T ( q ) dq = å a i ( q ) dq i = 0 (2.2.55)
i
The following condition from multivariable calculus determines whether a differential form with
two or more variables is exact (Lovelock and Rund, 1975):
Exact Differential Theorem: The differential form on the left side of Eq. (2.2.55) with
q Î R n , n ³ 2 , and continuously differentiable coefficients in an open set U Ì R n is
exact if and only if
45
¶a i ( q ) ¶a j ( q )
= (2.2.56)
¶q j ¶qi
for all i and j and all values of q Î U .
________________________________________________________________________
Example 2.2.7: The differential form in q Î R 3
a(q) = 2q 2 q 3dq1 + q1q 3dq 2 + q1q 2 dq 3
¶a1 ¶a
is not exact, since = 2q 3 ¹ q 3 = 2 . There is no need to test the remaining conditions of Eq.
¶q 2 ¶q1
(2.2.56), since if any condition fails for any i and j, the differential form is not exact.
________________________________________________________________________
Even if the differential form of Eq. (2.2.55) is not exact, there may be a scalar function
b(q) such that the differential form b(q )a T ( q ) dq is exact. Such a function is called an
integrating factor and the differential form aT ( q ) dq is said to be an integrable differential form.
A condition for the differential form of Eq. (2.2.55) to be integrable is as follows (Lovelock and
Rund, 1975):
Integrable Differential Theorem: The differential form of Eq. (2.2.55) with
q Î R n , n ³ 3, and continuously differentiable coefficients a i (q ) in an open set U Ì R n
is integrable if and only if
æ ¶a ¶a ö æ ¶a ¶a g ö æ ¶a g ¶a b ö
ag ç b - a ÷÷ + a b çç a - ÷÷ + a a çç - ÷÷ = 0 (2.2.57)
ç ¶q
è a ¶q b ø è ¶q g ¶q a ø è ¶qb ¶q g ø
for all 1 £ a < b < g £ n and for all values of q Î U . If the differential form of Eq.
(2.2.55) satisfies Eq. (2.2.57), there exists an integrating factor b(q) ¹ 0 such that the
differential form b(q )a ( q ) dq is exact.
T
The reason for the condition "in three or more variables" is that any differential form in
two variables is integrable (Lovelock and Rund, 1975). For k = 3, there is only one condition to
evaluate in Eq. (2.2.57). For k > 3, however, there are k(k - l)(k - 2)/6 conditions, which
becomes oppressive for k large. Even for k = 4, there are four conditions. The integrable
differential theorem may assure existence of an integrating factor, but it does not show how to
construct one.
________________________________________________________________________
Example 2.2.8: Applying the test of Eq. (2.2.57) to the differential form of Example 2.2.6 yields
q1q 2 (q 3 - 2q 3 ) + q1q 3 (2q 2 - q 2 ) + 2q 3q 2 (q1 - q1 ) = -q1q 2 q 3 + q1q 2 q 3 = 0 (2.2.58)
Thus, the differential form of Example 2.2.7 is integrable, even though it is not exact.
________________________________________________________________________
46
2.2.7 Newton-Raphson Method for Solution of Nonlinear Equations
At the intersection of multivariable calculus and matrix theory are iterative methods for
solving nonlinear algebraic equations. Let x be an n-vector of unknowns and
F(x ) = 0 Î R n (2.2.59)
be a system of n nonlinear equations to be solved for x, where the vector function has continuous
derivatives and the Jacobian matrix Fx (x ) is nonsingular in an open set D of R n . With an
estimate x 0 Î D of the solution, a first order Taylor expansion that seeks a perturbation Dx 0 of
x0 that better approximates the solution as x1 = x 0 + Dx 0 is
F(x 0 ) + Fx (x 0 )Dx 0 = 0
After j iterations, the incremental equation is
Fx (x j )Dx j = - F(x j )
(2.2.60)
x j+1 = x j + Dx j
which is continued until F(x j+1 ) £ Tol , where Tol is a solution error tolerance.
This is the Newton-Raphson method (Atkinson, 1989) for solution of Eq. (2.2.59). The
method has the attractive property that if x 0 is sufficiently close to the solution, the method
converges quadratically to the solution; i.e.,
2
x - x j+1 £ k x - x j (2.2.61)
where x is the solution and k is a constant. However, a good initial estimate is often difficult to
obtain, and the method may diverge for a poor initial estimate. Fortunately, in kinematic and
dynamic simulation applications on a time grid t i+1 = t i + h , i=1,2, ... , with h small, the solution
at t i can be used as the initial estimate at t i +1 and convergence is likely.
MATLAB Code 2.2 that implements the Newton-Raphson method is presented in Section
2.B.1 of Appendix 2.B.
2.2.8 Matrix Condition Number
The Jacobian Fx ( x ) being nonsingular is not adequate to assure good performance of the
Newton-Raphson method. The condition number of the Jacobian plays a crucial role (Strang,
1980). With the norm of a vector defined in Eq. (2.2.45), the matrix norm of the Jacobian is
Fx ( x ) M
= max x ¹ 0 ( Fx ( x )x M
/ x ) (2.2.62)
and the condition number of the Jacobian is defined as (Strang, 1980)

c = Fx ( x ) M
Fx ( x )-1 (2.2.63)
M
Fortunately, there are methods to obtain good approximations of the condition number, without
calculation of the inverse (Atkinson, 1989).
47
To see the significance of the condition number, consider the case in which the error in
evaluating the right side of the matrix equation Ax = b is db . A bound on the magnitude of the
perturbation δx in the resulting solution is (Strang, 1980)
δx / x £ c ( δb / A M ) (2.2.64)
If the condition number c of matrix A is small, the solution is not significantly affected by the
perturbation in the right side. If c is large, however, large error and possibly divergence can
occur. Similarly, if the matrix is perturbed by δA , a bound on solution error is (Strang, 1980)
δx / x + δx £ c ( δA M
/ A M ) (2.2.65)
As above, if the condition number c is small, the solution is not significantly affected by the
perturbation in the Jacobian. If c is large, however, large error and possibly divergence can
occur.
The condition number of a matrix A in an equation Ax = b thus serves as a check on
accuracy of solution. If c = A M A-1 M is large, problems occur. Unfortunately, if A is a scalar,
A M
= A and A -1 = 1 / A . Thus, c = A / A = 1 and no information is obtained. In this
M
case, c º 1 / A serves as a check on accuracy of the solution. If 1 / A is large, problems arise.

This serves as an extension of condition number when A is a scalar.
As may be noted in MATLAB code presented in the text for iterative solution of
nonlinear equations, the condition number is used as a warning of impending divergence of the
Newton-Raphson algorithm.
Matrix algebra and multivariable calculus provide indispensable tools for mechanical system
kinematics and dynamics. Both are ideally suited for digital computation. While the Lagrange
multiplier theorem of linear algebra has a somewhat abstract proof, it is easily applied as a
basic tool of multibody dynamics.
The Euclidean space R n , with the definition of scalar product, norm, and distance between
vectors, is indispensable in definition of continuity of functions and is the basis for differential
geometry, the foundation of kinematics and dynamics of mechanical systems.
The implicit and inverse function theorems and criteria for exactness and integrability of
differential forms are key in assuring existence of solutions of nonlinear equations and
understanding the difference between algebraic and differential kinematic constraints. Finally,
the Newton-Raphson iterative solution algorithm and the matrix condition number play pivotal
roles in numerical solution of equations of kinematics and dynamics.
48
Key Formulas
(A ) = (A )
-1
( AB ) ( AB )
-1 T -1
= BT AT = B -1 A - 1
T T
(2.2.16) (2.2.22) (2.2.23)
¶a é ¶a ù ¶f é ¶f i ù
aq º =ê ú fq º =ê ú (2.2.34) (2.2.35)
¶q êë ¶q j úû1×k ¶q êë ¶q j úû n×k
(g h)
T
q
= hT g q + g Thq ( F ( g (q)) ) q
= F g gq (2.2.36) (2.2.37)
{
R n = q = [q1 L q n ] : q i real, i = 1,L, n
T
} (2.2.41)
n
r, s º r T s = å ri si d (r , s ) º r - s n
qnº q, q (2.2.43) (2.2.44) (2.2.45)
1=1
{
B e (q 0 ) = q Î R n : q - q 0 < e } (2.2.47)
49
2.3 Position and Orientation of a Body in a Plane
A planar rigid body has constant thickness normal to a plane and mass that is distributed
in the plane. It is located and oriented in the plane by an orthogonal x ¢-y¢ reference frame that is
attached to the body. Care is taken to show that the body has just one orientation degree of
freedom and that it is the angle of rotation from the x-axis to the x ¢ -axis.
2.3.1 Reference Frames for Location and Orientation in a Plane
Consider a point P that is fixed in a right hand orthogonal x ¢-y¢ body fixed reference
frame that translates and rotates relative to a right hand orthogonal x-y global reference frame, as
r
shown in Fig. 2.3.1. Using geometric vector notation, with the vector s P fixed in the x ¢-y¢ frame
locating point P in the x ¢-y¢ frame, point P may be located relative to the x-y frame by the vector
equation
rP r rP
r = r +s (2.3.1)
y
y
y¢ P
x¢
rP
s
rP
r x
r
r
Figure 2.3.1 Vector Location of Point in Moving Reference Frame

In order to quantify the effect of rotation of a reference frame in the plane, consider the
r
x ¢-y¢ and x-y orthogonal frames with common origin shown in Fig. 2.3.2. The vector s shown
can be represented using its components relative to unit vectors in the two reference frames as
r r r
s = sx i + sy j (2.3.2)
where
r r r r
s x = s × i, s y = s × j (2.3.3)
and
r r r
s = s x ¢ f + s y¢ g (2.3.4)
where
50
r r r r
s x¢ = s × f, s y¢ = s × g (2.3.5)
y¢
x¢
r
s
r
j
r r
g f
r x
i
Figure 2.3.2 Orthogonal Reference Frames with Common Origin
r
Algebraic vectors that define s in the two frames are
T
s = éës x s y ùû (2.3.6)
in the x-y frame and

T
s¢ = éës x ¢ s y ¢ ùû (2.3.7)
in the x ¢-y¢ frame.

It is clear that there is a relation between s and s¢ , since they are defined by the same
r r r
geometric vector s . To establish this relation, the f and g unit vectors in the x ¢-y¢ frame may be
r r
written in terms of the i and j unit vectors in the x-y frame as
r r r
f = a11 i + a 21 j
r r r (2.3.8)
g = a12 i + a 22 j
where a ij are the direction cosines
r r rr r r r
a11 = i × f = cosθ ( i,f ) , a12 = i × g = cosθ( i,gr )
r r rr r r r (2.3.9)
a 21 = j × f = cosθ ( j,f ) , a 22 = j × g = cosθ( j,gr )
Substituting from Eq. (2.3.8) into Eq. (2.3.4) yields
r r r
s = ( a11s x ¢ + a12 s y¢ ) i + ( a 21s x¢ + a 22 s y¢ ) j (2.3.10)
r r r
Equating coefficients of i and j in this representation of s with those in Eq. (2.3.2),
51
s x = a11s x ¢ + a12s y¢
(2.3.11)
s y = a 21s x ¢ + a 22s y¢
In matrix form, this is
s = As ¢ (2.3.12)
where A is called the direction cosine matrix or orientation transformation matrix,
é a11 a12 ù
A=ê ú (2.3.13)
ëa 21 a 22 û
The orientation transformation matrix A has very special properties. If the x-y
r r
component forms of unit vectors f and g are denoted by f and g and the x-y components of unit
r r
vectors i and j are denoted by i and j, then
é1ù é0ù
i = ê ú, j= ê ú (2.3.14)
ë0û ë1û
and Eq. (2.3.8) shows that
é a11 ù é a12 ù
f = ê ú, g = ê ú (2.3.15)
ë a 21 û ë a 22 û
Therefore, the matrix A of Eq. (2.3.13) can be written as
A = [f g] (2.3.16)
Expanding the matrix product,

éf T ù é f Tf f Tg ù
A T A = ê T ú [f g] = ê T ú (2.3.17)
ëg û ëg f gTg û
Since the unit vectors f and g are orthogonal, this is
ATA = I (2.3.18)
Thus, A T = A -1 , and the orientation transformation matrix A is an orthogonal matrix. This
special property permits an easy solution of Eq. (2.3.12) ,
s¢ = A T s (2.3.19)
Transforming between algebraic vectors that represent the same physical vector in the x-y and
x ¢-y¢ Cartesian reference frames is thus a trivial matter.
The vector that locates a fixed point P in the x ¢-y¢ frame in Fig. 2.3.1, written in terms of
geometric vectors in Eq. (2.3.1), can now be written in algebraic form as
r P = r + s P = r + As¢P (2.3.20)
52
2.3.2 Orientation Degrees of Freedom
The four direction cosines in matrix A define the orientation of the x ¢-y¢ frame relative to
the x-y reference frame, but they are not independent. In component form, Eq. (2.3.18) is
2
ì 1, i=j
åa a kj = í , i, j=1, 2 (2.3.21)
î 0, i ¹ j
ki
k=1
Since interchanging i and j yields the same equation, Eq. (2.3.18) provides just three equations
among the four direction cosines.
To show that there is one degree of freedom of rotation in the plane, it must be shown
that Eq. (2.3.18), equivalently Eq. (2.3.21), imposes three independent constraints on the four
components of A. First note that, by Eq. (2.3.16), the four components of A are the components
of vectors f and g . Define the four variables comprising components of A as
éf ù
q=ê ú
ëg û
The constraints imposed on these variables by the diagonal and upper triangular parts of Eq.
(2.3.18), using the expansion of Eq. (2.3.17), may be written in the form
é 12 ( f Tf - 1) ù
ê ú
Φ ( q ) = ê 12 ( g T g - 1) ú = 0 (2.3.22)
ê ú
ê f Tg ú
ë û
T
Let q 0 = éëf 0T g 0T ùû satisfy Eq. (2.3.22), so A 0 = éëf 0 g 0 ùû is orthogonal. Then, the
Jacobian of Eq. (2.3.22) , evaluated at q 0 , is
é f 0T 0 ù
ê ú
Φq ( q 0 ) = ê 0 g 0T ú (2.3.23)
ê g 0T f 0T úû
ë
The three rows of this Jacobian are linearly independent; i.e., it is of full row rank, if the three
columns of its transpose are linearly independent; i.e., if the equation
éf 0 0 g0 ù
Φ (q ) u = ê
T
q
0
úu = 0 (2.3.24)
ë0 g0 f0û
implies u = [ u1 u 3 ] = 0 . Expanding the product of Eq. (2.3.24),

T
u2
éu ù éu ù
f 0 u1 + g 0 u 3 = ëéf 0 g 0 ûù ê 1 ú = A 0 ê 1 ú = 0
ë u3 û ë u3 û
(2.3.25)
éu ù éu ù
g 0 u 2 + f 0 u 3 = éëf 0 g 0 ùû ê 3 ú = A 0 ê 3 ú = 0
ëu 2 û ëu 2 û
53
Since A 0 is orthogonal, hence nonsingular, all components of u are zero and the Jacobian of Eq.
(2.3.23) has full row rank. The implicit function theorem thus shows that Eq. (2.3.22) can be
solved for three of its four variables as a function of the remaining independent variable. Thus,
there is one degree of freedom of rotation in the plane.
Define the rotation of the x ¢-y¢ frame relative to the x-y frame by angle f shown in Fig.
2.3.3, with counterclockwise taken as positive. Recalling the relations of Eq. (2.3.9) and using
the geometry of Fig. 2.3.3,
r r rr
( )
a11 = i × f = cosθ i,f = cos f
r r rr
( )
a12 = i × g = cosθ i,g = cos ( f + p / 2 ) = cos p / 2 cos f - sin p / 2 sin f = - sin f
r r rr (2.3.26)
( )
a 21 = j × f = cosθ j,f = cos ( p / 2 - f ) = cos p / 2 cos f + sin p / 2sin f = sin f
r r rr
( )
a 22 = j × g = cosθ j,g = cos f
This shows that the orientation matrix is

é cos f - sin fù
A ( f) = ê ú (2.3.27)
ë sin f cos f û
and the angle f shown in Fig. 2.3.3 is an independent generalized coordinate that defines
orientation of the x ¢-y¢ reference frame in the plane. This is consistent with the foregoing
analysis that showed there is one degree of freedom of rotation.
Figure 2.3.3 Relative Rotation of Planar Reference Frames

A trivial calculation shows that A ( f ) of Eq. (2.3.27) is orthogonal, for all f ; i.e.,
é cos f sin f ù é cos f - sin f ù

AT (f) A (f) = ê úê ú
ë - sin f cos f û ë sin f cos f û
é cos2 f + sin 2 f - cos f sin f + cos f sin f ù
=ê ú = I3
ë - cos f sin f + cos f sin f cos 2
f + sin 2
f û
2.3.3 Orientation Identities
Since A ( f ) transforms a vector that is fixed in the x ¢-y¢ frame to a vector in the
x-y frame, as a result of rotation of the x ¢-y¢ frame, it is called an orientation transformation
matrix. It has a number of special properties that are helpful in kinematic modeling of planar
54
systems. First, a vector v may be rotated counterclockwise by p / 2 to create a vector v ^ that is
perpendicular to v ,
é0 -1ù
v^ = A (p / 2) v = ê ú v º Pv (2.3.28)
ë1 0 û
That is,
é0 -1ù
P=ê ú (2.3.29)
ë1 0 û
Since P is an orientation transformation with f = p / 2 , it is orthogonal; i.e.,
PT P = I (2.3.30)
Direct manipulation shows that A and P have the following properties:
d
A ( f ) = PA ( f )
df
(2.3.31)
d T
A ( f) = -A T ( f ) P
df
PP = -I (2.3.32)
PA = AP
(2.3.33)
A T PA = P
PAP = - A (2.3.34)
PT AP = A (2.3.35)
PT A T P T = -A (2.3.36)
It is important to note that once an orientation transformation matrix A has been
determined to define the orientation of a planar body, the angle in Fig. 2.3.3 can be determined.
In particular, let
cos f = a11
(2.3.37)
sinf = a 21
and A ( f ) satisfy Eq. (2.3.18). Expanding A T A = I yields a11

2
+ a 21
2
= 1 in the (1,1) component,
which is required if Eq. (2.3.37) indeed defines trigonometric functions. Once sin f and cos f are
known, the value of f , 0 £ f £ 2π , may be uniquely determined by taking the Arcsin of both
sides of the second of Eqs. (2.3.37) and using the algebraic sign of cos f from the first of Eqs.
(2.3.37) to uniquely evaluate f . With
π π
- £ Arcsin a 21 £ (2.3.38)
2 2
a trigonometric analysis that is detailed in Section 2.4.4 shows that f is given by
55
ì-p - Arcsin a 21 , if a 21 £ 0 and a11 £ 0
ï
f = í Arcsin a 21 , if a11 ³ 0 (2.3.39)
ï p - Arcsin a , if a ³ 0 and a £ 0
î 21 21 11
When the origins of the x-y and x ¢-y¢ frames do not coincide, as shown in Fig. 2.3.1,
point P is located by
r P = r + A ( f ) s¢P (2.3.40)
where s¢P is fixed in the x ¢-y¢ frame. Thus, r and f define the location of all points in a planar
rigid body, so they may be regarded as generalized coordinates of the body. The generalized
coordinates of a planar rigid body are the three components of the vector of planar Cartesian
generalized coordinates,
ér ù
q=ê ú (2.3.41)
ëf û
2.3.4 Time Derivatives and Variations in Rotation and Displacement
In applications, an x ¢-y¢ Cartesian reference frame is fixed in a moving body to define its
position and orientation over time, relative to a stationary global x-y reference frame. Consider a
point P that is fixed in an x ¢-y¢ frame, as shown in Fig. 2.3.1. The vector that locates P in the x-y
frame is given by Eq. (2.3.40), where s¢P is the constant vector of coordinates of P in the x ¢-y¢
frame. Since the x ¢-y¢ frame is moving and changing its orientation with time, the vector
r = r (t) and the angle f = f(t) , hence the transformation matrix A = A(f(t)) , is a function of
time.
Differentiating both sides of Eq. (2.3.40) with respect to time and using Eq. (2.3.31)
yields the velocity of point P as
& ( f ) s¢P = r& + f& PA ( f ) s¢P
r& P = r& + A (2.3.42)
Taking the time derivative of Eq. (2.3.42) and using Eqs. (2.3.31) and (2.3.32) yields the
acceleration of point P as
r P = &&
&& fPA ( f ) s¢P + f& 2 PPA ( f ) s¢P
r + &&
(2.3.43)
fPA ( f ) s¢P - f& 2 A ( f ) s¢P
r + &&
= &&
The form of Eqs. (2.3.42) and (2.3.43) is characteristic of that to be encountered in spatial
kinematics. Velocity equations are linear in first derivatives of generalized coordinates, with
coefficients that depend on generalized coordinates. Acceleration equations are linear in second
derivatives, with coefficients that depend on generalized coordinates, but quadratic in first
derivatives of generalized coordinates. The quadratic velocity terms in the acceleration equations
are reminiscent of Coriolis terms that arise in classical equations of motion.
Taking the differential of Eq. (2.3.27), as with the derivative in Eq. (2.3.31),
56
d
dA ( f ) = A ( f ) df = dfPA ( f ) (2.3.44)
df
Similarly, taking the differential of both sides of Eq. (2.3.40) and using Eq. (2.3.44) yields
dr P = dr + dA ( f ) s¢P = dr + dfPA ( f ) s¢P (2.3.45)
where dr and df are differentials, or variations, of r and f .
The position and orientation of a body-fixed reference frame, relative to an inertial reference
frame, define the position and orientation of a planar body, using two position coordinates and a
single orientation coordinate. Algebraic vectors and a 2 ´ 2 orthogonal orientation transformation
matrix provide the basis for characterizing the position, orientation, velocity, and acceleration of
all points in the body. Identities involving orientation provide expressions for derivatives and
variations of coordinates of points to be used in digital computation.
Key Formulas
é cosf - sin f ù é 0 -1ù
A = A (f) = ê ú P = A (p / 2) = ê ú (2.3.27) (2.3.29)
ësin f cosf û ë1 0 û
s = As¢ PP = -I PA = AP (2.3.12) (2.3.32) (2.3.33)
d d T
A ( f ) = PA ( f ) A ( f) = -A T ( f ) P (2.3.31)
df df
r P = r + A ( f ) s¢P (2.3.40)
r& P = r& + f& PA ( f ) s¢P r P = &&

&& fPA ( f ) s¢P - f& 2 A ( f ) s¢P
r + && (2.3.42) (2.3.43)
dA ( f ) = dfPA ( f ) dr P = dr + dA ( f ) s¢P = dr + dfPA ( f ) s¢P (2.3.44) (2.3.45)
57
2.4 Position and Orientation of a Body in Space
A rigid body in space is defined as being made up of a continuum of particles that are
constrained not to move relative to one another. While actual bodies are never perfectly rigid,
deformation effects are often negligible when considering the motion of a machine that is
comprised of multiple bodies. For this reason, a significant component of the study of kinematics
and dynamics of machines is devoted to modeling individual bodies in a system as being rigid.
Relations that define the position and orientation of a rigid body in space for multibody
kinematics and dynamics are derived in this section, using the approach introduced in Section 2.3
for planar bodies. The matter of representing orientation in space, however is much more
intricate.
2.4.1 Reference Frames for Location and Orientation in Space
The geometry of the body shown in Fig. 2.4.1 may be defined in an x ¢-y ¢-z ¢ Cartesian
reference frame that is fixed to the body, called a body-fixed reference frame. A point P may be
defined in the body by a vector with algebraic representation s¢ P relative to the x ¢-y ¢-z ¢ frame.
Representing the same vector as sP in the global x-y-z reference frame, point P may be located
in the global frame by the vector relation
r P = r + sP (2.4.1)
Thus, data that define the geometry of the body are specified in the x ¢-y ¢-z ¢ frame and
transformed by vector relations into the x-y-z frame.
Figure 2.4.1 Body in Space with General Orientation

Translation of a body is defined by the vector r from the origin of the x-y-z reference
frame to the origin of the x ¢-y ¢-z ¢ frame. Specification of orientation is, however, more
complex. Nevertheless, once the x ¢-y ¢-z ¢ frame is oriented relative to the x-y-z reference frame,
all points of interest in the body can be defined in the x-y-z frame. Most of the attention in this
section focuses on the transformation from vectors that are represented in a body-fixed x ¢-y ¢-z ¢
frame to vectors represented in the x-y-z reference frame.
To see that the orientation of a body in space raises delicate technical questions, consider
two rotation sequences for a body from the reference orientation shown in Fig. 2.4.2.
Counterclockwise rotations of the body by angles of π/2 are successively made about its
58
body-fixed x¢ and y¢ axes. Consider first the rotation sequence shown in Fig. 2.4.3. The first
rotation is about the body-fixed x¢ axis, which initially coincides with the global reference
frame x axis. The second rotation is about the y¢ axis, as it coincides with the global reference
frame z axis, resulting in the orientation shown on the right of Fig. 2.4.3.
Figure 2.4.2 Rectangular Body in Reference Orientation
Figure 2.4.3 Rotation of Body by π/2 About x¢ Axis and Then by π/2 About y¢ Axis
Consider next the same rotations about body-fixed x¢ and y¢ axes, but in reverse order,
as shown in Fig. 2.4.4. First, the body is rotated π/2 about its body-fixed y¢ axis, which initially
coincides with the global reference frame y axis. The body is then rotated π/2 about its body-
fixed x¢ axis, as it coincides with the negative global reference frame z axis, resulting in the
orientation shown on the right of Fig. 2.4.4.
Figure 2.4.4 Rotation of Body by π/2 About y¢ Axis and Then by π/2 About x¢ Axis
Since the orientations shown on the right of Figs. 2.4.3 and 2.4.4 are distinctly different,
it is clear that the order of rotation is important in defining the orientation of a body in space.
59
Much as in the case of matrix multiplication, in which the order of terms in the product of two
matrices is critically important, the order of rotations in space is likewise important. This shows
that large amplitude rotation is not a vector quantity, since the order of rotations is not
commutative; i.e., orders of rotation cannot be interchanged.
2.4.2 Transformation of Coordinates
It is shown in Section 2.1 that a geometric vector is uniquely represented by an algebraic
vector that contains components of the geometric vector in a Cartesian reference frame. The
components of a vector, however, are defined in a specific Cartesian reference frame. Consider a
Cartesian x ¢-y ¢-z ¢ frame with the same origin as the x-y-z frame, as shown in Fig. 2.4.5. Unit
r r r
x ¢, y ¢, and z ¢ coordinate vectors are denoted by f , g , and h , respectively, and unit x, y, and z
r r r r
coordinate vectors are denoted by i , j , and k . A vector s in space can be represented in either
of the frames as
r r r r
s = s x i + s y j + sz k (2.4.2)
or
r r r r
s = s x ¢f + s y¢g + sz ¢ h (2.4.3)
where
r r r r r r
s x = s × i, s y = s × j, sz = s × k (2.4.4)
and
r r r r r r
s x ¢ = s × f, s y¢ = s × g, sz ¢ = s × h (2.4.5)
Figure 2.4.5 Cartesian Reference Frames with Common Origin

r
Algebraic vectors that define s in the x-y-z and x ¢-y ¢-z ¢ frames are
T
s = éës x sy s z ùû (2.4.6)
T
s¢ = éës x ¢ s y ¢ s z¢ ùû (2.4.7)
60
It is clear that there is a relation between s and s¢ , since they are defined by the same geometric
r r r r
vector s . To establish this relation, the f, g, and h unit vectors may be written in terms of the
rr r
i, j , and k unit vectors as
r r r r
f = a11 i + a 21 j + a 31k
r r r r
g = a12 i + a 22 j + a 32 k (2.4.8)
r r r r
h = a13 i + a 23 j + a 33k
where a ij are the following direction cosines:
r r rr r r rr r r rr
( ) ( ) ( )
a11 = i × f = cosθ i,f , a12 = i × g = cosθ i,g , a13 = i × h = cosθ i,h
( ) ( ) ( )
a 21 = j × f = cosθ j,f , a 22 = j × g = cosθ j,g , a 23 = j × h = cosθ j,h (2.4.9)
( ) ( ) ( )
a 31 = k × f = cosθ k,f , a 32 = k × g = cosθ k,g , a 33 = k × h = cosθ k,h

r
r
(
s = a 11s x ¢ + a12s y ¢ + a13s z ¢ i )
r
(
+ a 21s x ¢ + a 22s y¢ + a 23s z ¢ j ) (2.4.10)
r
(
+ a 31s x ¢ + a 32s y ¢ + a 33s z ¢ k )
rr r r
Equating coefficients of i, j, and k in this representation of s with those in Eq. (2.4.2) ,
s x = a11s x ¢ + a12 s y ¢ + a13s z¢
s y = a 21s x ¢ + a 22s y ¢ + a 23s z ¢ (2.4.11)
s z = a 31s x ¢ + a 32s y ¢ + a 33s z ¢
In matrix form, this is

s = As ¢ (2.4.12)
where A is the spatial direction cosine matrix, or orientation transformation matrix,
é a11 a12 a13 ù
A = êê a 21 a 22 a 23 úú (2.4.13)
êë a 31 a 32 a 33 úû
The orientation transformation matrix A has very special properties. If the x-y-z
r r r
component forms of unit vectors f, g, and h are denoted by f, g, and h and the x-y-z
rr r
components of unit vectors i, j, and k are denoted by i, j, and k, then
61
é1ù é0ù é0ù
i = 0 , j = 1 , k = ê0ú
ê ú ê ú (2.4.14)
ê ú ê ú ê ú
ëê 0 ûú ëê 0 ûú êë 1 ûú
and Eq. (2.4.8) shows that
é a11 ù é a12 ù é a13 ù
f = ê a 21 ú , g = ê a 22 ú , h = êê a 23 úú
ê ú ê ú (2.4.15)
êë a 31 úû êë a 32 úû êë a 33 úû
Therefore, the matrix A of Eq. (2.4.13) can be written as

A = [f g h] (2.4.16)
As can be verified by direct calculation,
fx gx hx
A = fy gy h y = fx ( g yh z - gz h y ) + f y ( g z h x - g x h z ) + fz ( g x h y - g y h x )
fz gz hz
é 0 -g z gy ù
ê ú
= f ê gz
T
0 -g x ú h = f T gh
%
ê -g y gx 0 úû
ë
r r r
Since the vectors f, g, and h form a right-hand Cartesian reference frame, f = g ´ h , or in
component form, f = gh % . Thus,
A = f T gh
% = f Tf = 1 (2.4.17)
Expanding the matrix product AAT yields

éf T ù é f Tf f Tg f Th ù
ê ú ê ú
A T A = ê gT ú [ f g h] = ê g Tf g T g g Th ú (2.4.18)
êhT ú êh T f h T g hT h úû
ë û ë
Since the unit vectors f, g, and h form an orthogonal triad, this is
AT A = I (2.4.19)
Thus, A T = A -1 and the orientation transformation matrix A is an orthogonal matrix. This
special property permits an easy solution of Eq. (2.4.12) ,
s ¢ = A Ts (2.4.20)
Transforming algebraic vectors that represent the same geometric vector in the x-y-z and
x ¢-y ¢-z ¢ Cartesian reference frames is thus a trivial matter, once matrix A is known.
62
When the origins of the x-y-z and x ¢-y ¢-z ¢ frames do not coincide, the foregoing analysis
is applied between the x ¢-y ¢-z ¢ frame and a translated x-y-z frame, as shown in Fig. 2.4.1. If the
algebraic vector s¢ P locates point P in the x ¢-y ¢-z ¢ frame, then in the translated x-y-z frame this
vector is
s P = As¢P (2.4.21)
Thus,
r P = r + sP = r + As ¢ P (2.4.22)
where r is the vector from the origin of the x-y-z frame to the origin of the x ¢-y ¢-z ¢ frame, as
shown in Fig. 2.4.1.
2.4.3 Orientation Degrees of Freedom
The nine direction cosines in matrix A define the orientation of the x ¢-y ¢-z ¢ frame,
relative to the x-y-z reference frame, but they are not independent. In component form, Eq.
(2.4.19) is
3
ì 1, i=j
åa a kj = í , i, j=1, 2, 3 (2.4.23)
î 0, i ¹ j
ki
k=1
Since interchanging i and j yields the same equation, Eq. (2.4.19) provides just six equations
among the nine direction cosines.
To show that there are three orientation degrees of freedom in space, it must be shown
that Eq. (2.4.19), equivalently Eq. (2.4.23), imposes six independent constraints on the nine
components of A. First note that by Eq. (2.4.16), the nine components of A are the components
of vectors f , g, and h , so the nine variables comprising components of A are
éf ù
q = êg ú
ê ú
ëêh ûú
The constraints imposed on these variables by the diagonal and upper triangular parts of Eq.
(2.4.19), using the expansion of Eq. (2.4.18), may be written in the form
é 12 ( f T f - 1) ù
ê ú
ê 12 ( g T g - 1) ú
ê ú
ê 12 ( h T h - 1) ú
Φ (q ) = ê ú=0 (2.4.24)
ê f g ú T
ê ú
ê f h ú
T
êë g T h úû
63
T
Let the 9-vector q 0 = éëf 0T g 0T h 0T ùû satisfy the six constraint equations of Eq.
(2.4.24). The Jacobian of the constraint function F ( q ) of Eq. (2.4.24), with the superscript 0
suppressed for notational convenience, is
éf T 0 0 ù
ê ú
ê0 g 0ú
T
ê 0 0 hT ú
Fq = ê T ú
êg f 0ú
T
êh T 0 f T ú
ê ú
ëê 0 h g úû
T T
The rows of the 6 ´ 9 matrix F q are linearly independent if and only if the columns of the 9 ´ 6
matrix F Tq are linearly independent; i.e., if and only if
F Tq u = 0
implies that u = [ u1 L u 6 ] = 0 . Expanding the product on the left, this is

T
é u1 ù
êu ú
é ùê ú
2
f 0 0 g h 0
êu3 ú
F qT u = êê 0 g 0 f 0 h úú ê ú = 0
u
êë 0 0 h 0 f g úû ê 4 ú
ê u5 ú
ê ú
êë u 6 úû
Expanding and reordering variables yields
é u1 ù é u1 ù
[f g h ] ê u 4 ú = A êê u 4 úú = 0
ê ú
êë u 5 úû êë u 5 úû
éu4 ù éu4 ù
g h ] ê u 2 ú = A êê u 2 úú = 0
ê ú
[f
éu5 ù é u5 ù
g h ] ê u 6 ú = A êê u 6 úú = 0
ê ú
[f
Since A = [f g h ] is nonsingular, these three equations imply that all components of u are
zero, hence u = 0 . Thus, F q has full row rank and it has a 6 ´ 6 submatrix with nonzero
determinant. Defining the variables in q that correspond to columns of the nonsingular submatrix
64
as dependent, the implicit function theorem of Section 2.2 guarantees that these six variables can
be written as unique functions of the remaining three variables, in some neighborhood of q 0 .
Thus, there are indeed three orientation degrees of freedom in space.
While the nine direction cosines, subject to six constraints of Eq. (2.4.19) , could be
adopted as generalized coordinates that define orientation of the x ¢-y ¢-z ¢ frame, this is neither
practical nor convenient. Thus, other orientation generalized coordinates are sought.
It is fascinating and very important that a rigid body in the two-dimensional plane has
one orientation degree of freedom, whereas a rigid body in three-dimensional space has three
orientation degrees of freedom. As shown in Section 2.3, a single rotation angle generalized
coordinate unambiguously defines orientation of a planar body and yields very simple orientation
transformation relations. As will be seen throughout the remainder of this chapter, the search for
generalized coordinates that define orientation of a body in space is more problematic.
For over two centuries, it has been known that a body in space has three orientation
degrees of freedom. The search for a set of three globally valid generalized coordinates to
characterize orientation, however, has continued to be about as productive as the eternal search
for the "fountain of youth". In fact, in 1964 a proof published (Stuelpnagel, 1964) that there
exists no set of three orientation generalized coordinates that is globally valid; i.e., without
singularities. This shows that singularities are unavoidable if one uses any possible set of three
orientation generalized coordinates. This remarkable paper, which is based on mathematical
topology that is beyond the reach of we mere mortal engineers, further shows that orientation
generalized coordinates of dimension four or more that introduce normalization constraints are
valid, without singularities. The difficulty here is not the result of a pathological orientation
transformation matrix, but with the mathematical impossibility to globally represent it with three
orientation generalized coordinates. This is analogous to the difficulty highlighted in Example
1.2.1 of Section 1.2.2 in parameterization of the unit sphere; i.e., there is no pair of singularity
free generalized coordinates that represents the entire surface of the smooth sphere.
________________________________________________________________________
Example 2.4.1: As a special case, consider the x-y-z and x ¢-y ¢-z ¢ frames shown in Fig. 2.4.6,
with z and z¢ axes that point out of the page coincident. The angle of rotation of the x¢ axis,
relative to the x axis, with counterclockwise as positive, is denoted as f .
Figure 2.4.6 Reference Frames with Coincident z and z¢ Axes

From Eq. (2.4.9),
65
a11 = a 22 = cosf
æπ ö
a 21 = cos ç - f ÷ = sinf
è2 ø
æπ ö
a12 = cos ç + f ÷ = -sinf
è2 ø
a 33 = 1
a 31 = a 32 = a13 = a 23 = 0
Thus, Eq. (2.4.12) yields the transformation
és x ù é cosf -sinf 0 ù és x ¢ ù
s = ês y ú = ê sinf cosf 0 ú ês y¢ ú º As¢ (2.4.25)
ê ú ê úê ú
êë s z úû êë 0 0 1 úû êë s z¢ úû
which agrees with the planar relation in Eq. (2.3.27).

______________________________________________________________________________________
2.4.4 Identities Involving the Orientation Transformation Matrix
Useful relations may be obtained by noting that, for any vectors s¢ and v¢ in the x ¢-y ¢-z ¢
frame, the vector product of s and v in the x-y-z frame may be obtained by forming the vector
product of s¢ and v¢ in the x ¢-y ¢-z ¢ frame and transforming the result to the x-y-z frame; i.e.,
% = A ( s% ¢v ¢ )
sv
Using v = Av ¢ , this is
s% ( Av ¢ ) = ( sA
% ) v ¢ = ( As% ¢ ) v ¢
Since this must hold for arbitrary vectors v¢ ,

% = As%¢
sA (2.4.26)
Substituting s = As¢ , multiplying on the right by A T , and using Eq. (2.4.19) yields
±¢ = As%¢A T
s% = As (2.4.27)
for arbitrary s¢ . Similarly,
²
s% ¢ = A T
s = A T sA
% (2.4.28)
for arbitrary s .
Consider the pair of Cartesian x i¢-yi¢-z i¢ and x ¢j -y ¢j -z ¢j frames shown in Fig. 2.4.7. An
arbitrary vector s in the x-y-z frame has representations s¢i and s¢j in the x i¢-yi¢-z i¢ and x ¢j -y ¢j -z ¢j
frames, respectively; i.e.,
s = Ai si¢ = A js ¢j (2.4.29)
66
where A i and A j are transformation matrices from the x i¢-yi¢-z i¢ and x ¢j -y ¢j -z ¢j frames to the
x-y-z frame, respectively. Since A i and A j are orthogonal matrices, Eq. (2.4.29) yields
s i¢ = A iT A js ¢j º A ijs ¢j (2.4.30)
Since s¢j is an arbitrary vector,
A ij = A iT A j (2.4.31)
is the transformation matrix from the x ¢j -y ¢j -z ¢j frame to the x i¢-yi¢-z i¢ frame. A direct calculation
shows that A ij is an orthogonal matrix.
z
y ¢j s
zi¢ y i¢
y
z ¢j
x i¢
x ¢j
x
Figure 2.4.7 Reference Frames with Coincident Origins

In numerous applications, kinematic constraints dictate that certain axes of the x i¢-yi¢-z i¢
and x ¢j -y ¢j -z ¢j frames remain parallel; e.g., the unit vectors hi and h j shown in Fig. 2.4.8 are to
be collinear, with hi = h j . The angle qi j of rotation, measured positive as counterclockwise from
fi to f j , is to be calculated.
Figure 2.4.8 Triads With z¢ Axes Coincident
67
From the definition of scalar product and the fact that the coordinate vectors shown in
Fig. 2.4.8 are unit vectors,
fiT f j = cosqi j (2.4.32)
Similarly, from the definition of vector product,

f%i f j = hi sinqi j (2.4.33)
Taking the scalar product of both sides of Eq. (2.4.33) with hi and using the fact that f%i hi = - gi ,
sinqi j = hTi f%i f j = g Ti f j (2.4.34)
Writing the unit vectors in terms of the respective reference frames in which they are fixed and
using the transformation matrices from these frames to the x-y-z frame, Eq. (2.4.32) becomes
cosqi j = fi¢T A iT A jf j¢ (2.4.35)
Similarly, Eq. (2.4.34) becomes

sinqi j = g¢i T AiT A jf j¢ (2.4.36)
To simplify notation, define c º cosqi j = fi¢T A iT A jf j¢ and s º sinqi j = g¢i T A iT A jf j¢ .
As seen in Fig. 2.4.9, in the interval [-p / 2 £ qi j £ p / 2] , c ³ 0 and

qi j = Arcsin( g¢i T AiT A jf j¢) .
Figure 2.4.9 sin(x) and cos(x) on Interval [-p £ x £ p]

In the interval [-p £ qi j £ -p / 2] , c £ 0 and s £ 0 . Symmetry of the sin function about
-p / 2 yields sin(-p / 2 + f) = sin(-p / 2 - f) = - sin(p / 2 + f) . With qi j = -p / 2 - f , f = -p2 - qi j
and s = sin( qi j ) = sin( -p / 2 - p / 2 - qi j ) = - sin( qi j + p) . Since 0 £ qi j + p £ p / 2 ,
qi j + p = Arcsin( -s) = - Arcsin(s) and qi j = -p - Arcsin( g¢i T AiT A jf j¢) .
68
In the interval [p / 2 £ qi j £ p] , c £ 0 and s ³ 0 . Symmetry of the sin function about p / 2
yields sin(p / 2 + f) = sin(p / 2 - f) . With qi j = p / 2 + f , f = qi j - p / 2 and
s = sin( qi j ) = sin( p / 2 + p / 2 - qi j ) = sin( p - qi j ) . Since 0 £ p - qi j £ p / 2 ,
p - qi j = Arcsin(s) and qi j = p - Arcsin( g¢i T AiT A jf j¢) .
In summary, with c º cosqi j = fi¢T A iT A jf j¢ and s º sinqi j = g¢i T A iT A jf j¢ known, the angle qi j
is uniquely determined in the interval [-p £ qi j £ p] as
ì-p - Arcsin( s ), if s £ 0 and c < 0

ï
qi j = í Arcsin(s ), if c ³ 0 (2.4.37)
ï p - Arcsin(s ), if s ³ 0 and c<0
î
which is implemented in MATLAB Code 2.4 in Section 2.B.2 of Appendix 2.B.
2.4.5 Velocity, Acceleration, Angular Velocity, and Angular Acceleration
An x ¢-y ¢-z ¢ frame is fixed in a moving body to define the body’s position and
orientation, relative to an x-y-z global reference frame. Consider a point P that is fixed in the
x ¢-y ¢-z ¢ frame, as shown in Fig. 2.4.1. The vector that locates P in the x-y-z reference frame is
given by Eq. (2.4.22),
r P = r + As ¢ P (2.4.38)
where s¢ P is the constant vector of coordinates of P in the x ¢-y ¢-z ¢ frame and A is the orientation
transformation matrix from the x ¢-y ¢-z ¢ frame relative to the stationary x-y-z frame.
Since the x ¢-y ¢-z ¢ frame is moving and changing its orientation with time, the vector r
and transformation matrix A are functions of time. The differentiation rules of Section 2.1 can be
used to obtain the time derivative of r P as
& ¢P
r& P = r& + As (2.4.39)
Using Eq. (2.4.20), this result may be rewritten as
& T sP
r& P = r& + AA (2.4.40)
To interpret terms in Eq.(2.4.40), it is helpful to derive an identity that involves the
orientation transformation matrix A. Differentiating both sides of AA T = I with respect to time,
& T + AA
AA &T =0 (2.4.41)
Thus,
& T = - AA
& T = - AA
& T ( )
T
AA (2.4.42)
& T is skew-symmetric. As noted following the definition of Eq. (2.1.22) in Section 2.1,
so AA
there exists a vector w such that w & T . Using Eq. (2.4.26), the body fixed representation of
% = AA
% ¢ = A T AA
this vector is w ( )
& T A = AT A& . Together, these relations are
69
w
% = AA& T
(2.4.43)
&
% ¢ = AT A
w
This is the definition of ω in terms of the transformation matrix A and its time derivative.
Substituting the first of Eqs. (2.4.43) into Eq. (2.4.40) yields the velocity equation
r& P = r& + w% s P (2.4.44)
In geometric vector notation, this is the familiar relationship
rP r r rP
&r = r& + ω ´ s (2.4.45)
r
where ω is the angular velocity of the x ¢-y ¢-z ¢ frame. Thus, the vector w defined by Eq.
(2.4.43) is the angular velocity of the x ¢-y ¢-z ¢ frame, relative to the x-y-z frame. Since the
vector product in Eq. (2.4.44) is with the vector sP represented in the x-y-z frame, the vector w
is also represented in the x-y-z frame.
It is important to recall that angular velocity is defined by Eq. (2.4.43), rather than a
physical argument based on rotation speed about some axis. Such arguments introducing angular
velocity are generally incorrect, other than a special case of rotation about an axis that is fixed in
space; i.e., not time dependent.
Since w is a vector represented in the x-y-z frame, it may also be represented in the
x ¢-y ¢-z ¢ frame as
w ¢ = AT w (2.4.46)
Taking the vector product called for in Eq. (2.4.44) in the x ¢-y ¢-z ¢ frame and transforming to the
x-y-z frame yields
r& P = r& + Aw% ¢s ¢ P (2.4.47)

________________________________________________________________________
Example 2.4.2: The transformation matrix A of Example 2.4.1 is defined in Eq. (2.4.25) as a
function of the angle f . For this matrix, Eq. (2.4.43) yields
é -sinf -cosf 0 ù é cosf sinf 0 ù

&
% = AA
ω T
= f& êê cosf -sinf 0 úú êê -sinf cosf 0 úú
êë 0 0 0 úû êë 0 0 1 úû
é0 -f& 0ù é 0 -ω z ω y ù
ê& ú ê ú
= êf 0 0ú º ê ωz 0 -ω x ú
ê0 0 0úû êë -ωy ω x 0 úû
ë
Thus,
ω = éë 0 0 f& ùû
T
which agrees with the physical concept of angular velocity about a fixed axis.
70
________________________________________________________________________
Multiplying both sides of the first of Eqs. (2.4.43) on the right by A yields the useful
relationship
& =w
A %A (2.4.48)
and on the right by A in the second of Eqs. (2.4.43) yields
& = Aw
A %¢ (2.4.49)
Equations (2.4.48) and (2.4.49) provide frequently used relationships between the time derivative
of the transformation matrix A and the angular velocity w of the x ¢-y ¢-z ¢ frame.
Equation (2.4.39) may be differentiated with respect to time to obtain
r P = &&
&& && ¢ P
r + As (2.4.50)
Differentiating Eq. (2.4.48) and using Eq. (2.4.48) , the following relationships for the second
time derivative of the transformation matrix A are obtained:
&& = ωA
A %& + ωA
%&
(2.4.51)
%& + ωωA
= ωA % %
& is angular acceleration of the x ¢-y¢-z¢ frame. Similarly, using Eq. (2.4.49),
where w
&& = Aw%& ¢ + Aw
A % ¢w
%¢ (2.4.52)
&&
r P = && &% P + ωωs
r + ωs % % P
(2.4.53)
= &&
r + Aω &% ¢s¢P + Aω% ¢ω
% ¢s¢P
The form of Eqs. (2.4.53) and (2.4.47) are identical to the form of Eqs. (2.3.23) and
(2.3.24), and is characteristic of what is to be encountered in the equations of kinematics and
dynamics. Velocity equations are linear in first derivatives of generalized coordinates and
angular velocities, with coefficients depending on generalized coordinates. Acceleration
equations are linear in second derivatives of generalized coordinates and angular acceleration,
with coefficients depending on generalized coordinates, but contain quadratic terms in first
derivatives of generalized coordinates and angular velocities, with coefficients that depend on
generalized coordinates. The quadratic velocity terms in the acceleration equations are
reminiscent of Coriolis terms that arise in classical equations of motion. These nonlinear effects
reinforce the arguments in Chapter 1 that mechanical systems are essentially nonlinear.
_______________________________________________________________________
Example 2.4.3: Using the results of Example 2.4.1 and Eq. (2.4.47), the velocity of a point P
that is defined by s ¢ P = [1 1 1] in the x ¢-y ¢-z ¢ frame is
T
% ¢s¢P = f& [ -cosf - sinf cosf - sinf 0]

T
r& P = Aω
where r& = 0 . The acceleration of point P, from Eqs. (2.4.50) and (2.4.51), is
71
é -cosf - sinfù é -cosf + sinfù
r = As¢ = f cosf - sinf + f -cosf - sinf ú
&&P && P && ê ú & 2 ê
ê ú ê ú
êë 0 úû êë 0 úû
________________________________________________________________________
Derivative relations derived here serve as the foundation for kinematic analysis of rigid
bodies in space. The reader is encouraged to become proficient in their use, in preparation for
dynamic analysis. As an aid in the use of the relations derived, a summary of key formulas is
provided at the end of the section.
2.4.6 Infinitesimal Rotation and Displacement
Much as in the case of angular velocity, the identity AA T = I may be manipulated using
the differential, rather than time derivative. In this setting, the differential may be interpreted as
an infinitesimal variation at a given instant in time; i.e., with time held fixed. Taking the
differential of both sides of the identity of Eq. (2.4.19) yields
δAA T + AδA T = O ( δA 2 ) (2.4.54)
where the term O ( δA 2 ) denotes infinitesimal quantities that approach zero faster than δA . In
terms of the matrix norm B = max bij , any matrix D in O ( δA 2 ) satisfies the property
i,j
æ D ö
lim ç ÷=0
dA ® 0 ç δA ÷
è ø
In the sense of infinitesimals, the right side of Eq.(2.4.54) is said to be zero; i.e.,
δAA T + AδA T = 0 (2.4.55)
Thus,
δAA T = - AδA T = - ( δAA T )

T
(2.4.56)
and the matrix δAA T is skew symmetric.

Since any skew symmetric matrix can be written as the tilde operator applied to some
3-vector, there exists a vector denoted δπ such that
δAA T = δp% (2.4.57)
or,
δA = δp% A (2.4.58)
Taking the differential of the vector relationship a = Aa¢ , with a¢ constant,
δa = δAa¢ = δp% Aa¢ = δp% a (2.4.59)
Equation (2.4.59) shows that the vector product on the right is between δp and a . Since a is a
vector represented and the global reference frame, δp is also represented in the global reference
frame.
72
To see if the matrix A + δA = A + δp% A is orthogonal, as must be the case if the variation
defined by δp represents a rotation, expanding the product
( A + δp% A ) ( A + δp% A ) = A T A + A Tδp% A - A T δp% A - A T δp% δp% A

T
= I + ( δp% A ) δp% A = I + δA TδA

T
(2.4.60)
(
= I + O δA 2 )
( )
since the product δA T δA is O δA 2 . Thus, in the sense of infinitesimals, A + δA = A + δp% A is
indeed orthogonal and δp represents an infinitesimal rotation of the x ¢-y¢-z¢ frame.
In terms of the body fixed representation of infinitesimal rotation δp¢ , δp = Aδp¢ , and
δA = δp% A = Aδp% ¢A T A = Aδp% ¢ (2.4.61)
where the identity b% = Ab% ¢A T of Eq. (2.4.27) has been used with b = δπ . Thus, δp and δp¢ play a
role similar to that of angular velocity.
Taking the differential of the vector relation
r P = r + s P = r + As¢P (2.4.62)
and using Eq. (2.4.61) yields
δr P = δr + δAs¢P = δr + δp% s P = δr + Aδp% ¢s¢P (2.4.63)
where δr is regarded as an infinitesimal displacement.
In contrast to characterizing the orientation of a body in a plane, representation of orientation of a

body in space is at best intricate. The vector locating the origin of a body-fixed reference frame in
space is an easy extension of the planar case. For orientation, however, it is shown that the
transformation matrix from the body-fixed frame to the global frame is a 3 ´ 3 orthogonal
matrix. The implicit function theorem shows that there are three orientation degrees of freedom,
but does not identify three globally applicable orientation variables. This is because there are
none, as proven only in 1964 by Stuelpnagel.
Identities involving the orientation transformation matrix and its time derivative lead to the
definition of angular velocity and acceleration, each a three-dimensional vector. Vector and
differential calculus define the position, velocity, acceleration, and variation of all points in a
body, which are required for establishing equations of motion of bodies in space.
73
Key Formulas
s = As¢ s¢ = A T s ATA = I (2.4.12) (2.4.20) (2.4.19)
r P = r + s P = r + As¢P (2.4.22)
s% = As%¢A T s%¢ = A T sA
% (2.4.27) (2.4.28)
r& P = r& + ωs
% P % ¢s¢P
r& P = r& + Aw (2.4.44) (2.4.47)
& =w
A %A & = Aw
A %¢ (2.4.48) (2.4.49)
r P = &&
&& r - s% Pω
& + ωωs
% % P = && &% ¢s¢P + Aω
r + Aω % ¢ω
% ¢s¢P (2.4.53)
δA = δp% A δA = Aδp% ¢ (2.4.58) (2.4.61)
δr P = δr + δp% s P = δr + Aδp% ¢s¢P (2.4.63)
74
2.5 Euler Parameter Orientation Coordinates for a Body in Space
A set of four Euler parameters that satisfy a normalization condition is introduced to
define orientation of a body in space. Euler parameters are shown to avoid singularities that are
encountered with any set of three coordinates that are used to define orientation and to have
attractive analytical and computational properties.
2.5.1 Invariants of the Orientation Transformation
Using Eq. (2.4.19) and the notation a = (a a)

T
for length of a vector,
a = a Ta = a¢T A T Aa¢ = a¢T a¢ = a¢

2 2
(2.5.1)
Thus, orientation transformation preserves length of a vector; i.e.,

a = Aa¢ = a¢ (2.5.2)
Using this result and the scalar product relation of Eq. (2.1.20),
r r
a × b = a b cosq ( a,b ) = a Tb = a¢T A T Ab¢
r r (2.5.3)
= a¢Tb¢ = a¢ b¢ cosq ( a¢,b¢ ) = a ¢ × b¢
Thus, scalar product is preserved and
cosq ( a,b ) = cosq ( a¢,b¢ ) (2.5.4)
so orientation transformation preserves the angle between vectors.

An important property of the orientation transformation matrix A, shown in Eq. (2.4.17),
is that its determinant is unity; i.e.,
A =1 (2.5.5)
Direct manipulation yields the following relationship:
( A - I ) AT = A T A - A T = I - AT = ( I - A ) = -(A - I)
T T
(2.5.6)
Key facts (Kreyszig, 2011) regarding determinants are that the determinant of a product of
square matrices A and B is the product of their determinants; i.e., AB = A B , the determinant
of the transpose of a matrix is its determinant, and the determinant of the negative of a 3 ´ 3
matrix is ( -1) = -1 times its determinant. Using these facts and Eq. (2.5.6), for any orthogonal
3
matrix A with unit determinant,
A - I = A - I A T = ( A - I ) A T = I - A T = - ( A - I ) - ( A - I ) = - A - I (2.5.7)
T
Thus, A - I = 0 . By Eq. (2.2.20), there exists a unit vector u such that ( A - I ) u = 0 , or
Au = u (2.5.8)
By definition, Au¢ = u and Eq. (2.5.8) shows that
75
u = u¢ (2.5.9)
Thus, the transformation A leaves the vector u unchanged, so the x-y-z frame may be brought
into coincidence with the x ¢-y¢-z¢ frame by a rotation by an angle χ about the vector u, as shown
in Fig. 2.5.1. This result may be interpreted geometrically in the form of Euler’s theorem (Palais,
Palais, and Rodi, 2009).
Euler's Theorem: If the origins of two right-hand Cartesian reference frames
coincide, they may be brought into coincidence by a single rotation about some
axis.
Figure 2.5.1 Euler Rotation

2.5.2 Orientation of Reference Frames
As shown in Fig. 2.5.2, the Euler rotation transforms the unit vector i on the surface of a
cone until it comes into coincidence with unit vector f. Since cosq ( u,i ) = cosq ( u,f ) , Since the
ends of f and i lie in a plane that is orthogonal to u, as shown in Fig. 2.5.2, both f and i have the
same projection on u, given by
( ) ( )
v = i T u u = f Tu u (2.5.10)
Since i - v is rotated by the transformation to f - v and vector length is preserved by the

transformation,
i - v = f - v = sinq ( i,u ) = ui
% (2.5.11)
so i - v and f - v form radii of a circle in the plane orthogonal to u, as shown in Fig. 2.5.1.
76
Figure 2.5.2. Relation Between Transformed Vectors f and i
The objective is to relate unit vector i and its counterpart f in the x-y-z reference frame.
The vector v = (u T i)u is the intersection of u with the plane containing the endpoints of f and i.
The vector b perpendicular to i - v to the tip of vector f in Fig 2.5.1 is perpendicular to the plane
% . Further, b has length
of intersection of u and i, so it is parallel to ui
b = f - v sinχ = i - v sinχ . Equation (2.5.11) thus shows that b = ( ui % ) sinc . Using Eq.
(2.5.11), vector a has length a = f - v cos c = i - v cos c and since it is along vector i - v ,
( ( ))
a = ( i - v ) cosc = i - uT i u cosc . With these results, and using the geometry of Fig. 2.5.1 to
represent the vector sum v + a + b ,
( ) ( ( ) )
f = v + a + b = uT i u + i - u T i u cosc + ( ui
% ) sinc (2.5.12)
The same analysis can be used to relate g and j and h and k , obtaining
( ) ( ( ) )
g = uT j u + j - u T j u cosc + ( uj
% ) sinc
(2.5.13)
h = ( u k ) u + ( k - ( u k ) u ) cosc + ( uk
T T
% ) sinc
Substituting from Eqs. (2.5.12) and (2.5.13) into Eq. (2.4.16) yields
A = [f g h]
( ) u ( u j) u ( u k ) ùû
= (1 - cosc ) éëu u Ti T T
+ cosc [i j k ] + sincu% [i j k] (2.5.14)

= (1 - cosc ) uu T [i j k ] + coscI + sincu%
= (1 - cosc ) uu T + coscI + sincu%
________________________________________________________________________
Example 2.5.1: Consider the special case in which the vector about which rotation takes place is
u = [ 0 0 1] ; i.e., rotation occurs as the counterclockwise angle f about the z axis in Fig.
T
2.4.8. As shown in Example 2.4.2, the orientation transformation matrix is
77
é cosf - sin f 0 ù
A ( f ) = êsin f cosf 0 ú (2.5.15)
ê ú
êë 0 0 1 úû
Substituting u = [ 0 0 1] for this case into Eq. (2.5.14) yields

T
écos c - sin c 0ù
A ( c ) = ê sin c cos c 0ú (2.5.16)
ê ú
êë 0 0 1 úû
which is consistent with Euler's theorem and the angle of rotation c in Fig. 2.5.2.
________________________________________________________________________
2.5.3 Euler Parameters
Using the trigonometric identities
χ
1 - cosχ = 2sin 2
2
χ χ
sinχ = 2sin cos
2 2
(2.5.17)
χ
cosχ = 2cos 2 - 1
2
χ χ
cos 2 + sin 2 = 1
2 2
the orientation transformation matrix of Eq. (2.5.14) may be written as
χ T æ χ ö χ χ
A = 2sin 2 uu + ç 2cos2 - 1÷ I + 2sin cos u%
2 è 2 ø 2 2
(2.5.18)
χ æ χ χö χ χ
= 2sin 2 uu T + ç cos2 - sin 2 ÷ I + 2sin cos u%
2 è 2 2ø 2 2
Defining four Euler parameters as
χ
e0 º cos
2 (2.5.19)
χ
e º sin u
2
the orientation transformation matrix on the second line of Eq. (2.5.18) is written as
( )
A = e 20 - e Te I + 2ee T + 2e 0e% (2.5.20)
where the first and second terms in Eq. (2.5.20) are symmetric matrices and the third term is
skew-symmetric. In component form, Eq. (2.5.20) is
78
ée 20 + e12 - e22 - e32 2 ( e1e 2 - e0 e3 ) 2 ( e1e3 + e 0e 2 ) ù
ê ú
A = ê 2 ( e1e 2 + e0e3 ) e -e +e -e
2
0
2
1
2
2
2
3 2 ( e 2e3 - e 0e1 ) ú (2.5.21)
ê 2 ( e1e3 - e0e 2 ) 2 ( e 2e3 + e0e1 ) e02 - e12 - e 22 + e32 úû
ë
The quadratic form of terms in A, as functions of Euler parameters, will play a crucial role in
obtaining important identities and in efficient computation.
If the form of the orientation transformation matrix on the first line of Eq. (2.5.18) had
been used, the orientation transformation matrix could be written in terms of Euler parameters in
the equivalent form
( )
A = 2e0 - 1 I + 2 eeT + e 0e%
2
( ) (2.5.22)
This form of the orientation transformation matrix has been used in some textbooks (Wittenburg,
1977; Haug, 1989). It suffers, however, from requiring satisfaction of the Euler parameter
normalization condition, in order that a key factorization of A as a product of two matrices that
are linear in Euler parameters holds. This condition is not satisfied during iterative numerical
solution procedures, hence precluding use of the factorization in important applications. It is
interesting to note that the form of Eq. (2.5.20) is in fact used in some classical developments;
e.g., (Goldstein, 1980).
The 4-vector of Euler parameters is defined as
p = [ e0 e3 ] = éëe 0
T
eT ùû
T
e1 e 2 (2.5.23)
Equation (2.5.19) and u Tu = 1 yield the Euler parameter normalization condition,

χ χ χ χ
p Tp = e 02 + e Te = cos 2 + sin 2 uT u = cos 2 + sin 2 = 1
2 2 2 2
This relation and its time derivative yield the identities
p Tp = 1
(2.5.24)
p Tp& = 0
MATLAB Code 2.5 outlined in Section 2.5.6 and contained in Appendix 2.B implements
the Euler rotation definition of the orientation transformation matrix.
2.5.4 Mapping from the Orientation Transformation Matrix to Euler Parameters
Given an orthogonal transformation matrix A, it is often required to determine an
T
associated set of Euler parameters; i.e., a vector p = éëe 0 e T ùû such that A = A(p) . In particular,
in defining an initial orientation for a body, unit vectors f , g, and h can often be defined along
the body fixed x ¢-y¢-z¢ axes and the orientation transformation matrix for the body is given by
Eq. (2.4.16), A = [ f g h] . Euler parameters associated with this orthogonal matrix are then
required to initiate a simulation.
The trace of a given orthogonal transformation matrix A, denoted trA, is defined as the
sum of the diagonal elements of the matrix; i.e.,
79
trA º a11 + a 22 + a 33 (2.5.25)
Equating the sum of diagonal terms in the orientation transformation matrix of Eq. (2.5.21) to the
trace of the given orthogonal transformation matrix A and using the first identity of Eq. (2.5.24) ,
trA = 3e 20 - e12 - e 22 - e32 = 4e 02 - 1 (2.5.26)
Thus,
trA + 1
e02 = (2.5.27)
4
For any 3 ´ 3 orthogonal matrix A, -1 £ trA £ 3 (Bernstein, 2009), so 0 £ e 20 £ 1 .
To be precise, the method and theory of determining the transformation from A to Euler
parameters is stated as a theorem.
Inverse Euler Parameter Mapping Theorem: Let A be an orthogonal matrix with
T
A = 1 . There exist locally unique Euler parameters p = éëe 0 e T ùû , e0 = cos(c / 2) and
e = sin(c / 2)u , that are determined by Eq. (2.5.20), as follows:
(a) if A T ¹ A ; i.e., A is not symmetric (by far the most common case),
e0 = ± (1/ 2) trA + 1 ¹ 0
éa 32 - a 23 ù
æ 1 öê ú (2.5.28)
e = ±ç ÷ ê a13 - a 31 ú
è 2 trA + 1 ø ê
ë a 21 - a12 úû
where the same choice of sign is made in both expressions.
(b) if A T = A , e0 = ±1 , e = 0, and A = I .
Proof: In case (a), A is not symmetric, so the third term in Eq. (2.5.20) cannot be zero, so
e0 ¹ 0 . The vector u in Eq. (2.5.19) is the normalized solution of
Au = u (2.5.29)
Since A T = A -1 , Eq. (2.5.29) may be multiplied on the left by A T , yielding
A T Au = u = A Tu (2.5.30)
Subtracting Eqs. (2.5.29) and (2.5.30), u satisfies
(A - A )u = 0
T
(2.5.31)
Since ( A - A T ) = A T - A = - ( A - A T ) , A - A T is skew symmetric and can be written as

T
é 0 a12 - a 21 a13 - a 31 ù
A - A = ê a 21 - a12
T
0 a 23 - a 32 ú = b% (2.5.32)
ê ú
ëê a 31 - a13 a 32 - a 23 0 ûú
80
where
éa 32 - a 23 ù
b = ê a13 - a 31 ú ¹ 0 (2.5.33)
ê ú
ëê a 21 - a12 ûú
Equation (2.5.31) may thus be written as
% =0
bu (2.5.34)
As shown in Section 2.4, since b ¹ 0 ¹ u , u and b are parallel and u = ab for some real a .
Thus, a normalized solution of Eq. (2.5.29) is
u = (1/ b ) b (2.5.35)
Substituting e0 and u from Eqs. (2.5.27) and (2.5.35), Eq. (2.5.19) is written as
e0 = cos(c / 2) = ± trA + 1 / 2
(2.5.36)
e = sin(c / 2) (1 / b ) b
Subtracting symmetrically placed off-diagonal terms in the given matrix A and the matrix of Eq.
(2.5.21), equating results, and substituting from Eqs. (2.5.36) and (2.5.33) yields
a12 - a 21 = -4e 0 e3 = 4cos(c / 2)sin(c / 2) ( a12 - a 21 ) / b
a13 - a 31 = 4e0 e2 = 4cos(c / 2)sin(c / 2) ( a13 - a 31 ) / b (2.5.37)
a 23 - a 32 = -4e0 e1 = 4cos(c / 2)sin(c / 2) ( a 23 - a 32 ) / b
Since b ¹ 0 , Eq. (2.5.37) implies 4cos(c / 2)sin(c / 2) / b = 1 and, using Eq. (2.5.27),
sin(c / 2) = b / ( 4cos(c / 2) ) = ± b / 2 trA + 1 ( ) (2.5.38)
Substituting this result into Eq. (2.5.36),

e0 = ± trA + 1 / 2
( )
(2.5.39)
e = ± 1/ (2 trA + 1) b
which is Eq. (2.5.28).

In case (b), A T = A , so the third term in Eq. (2.5.20) must be zero and either
e0 = 0 or e = 0 . If e = 0 , e0 = ±1 and, since all off diagonal terms are zero, Eq. (2.5.21) shows
that A = I and p = ± [1 0 0 0] . If e0 = 0 , Eq. (2.5.27) implies trA = -1 . Equating diagonal
T
terms of A and the right side of Eq. (2.5.21),

a11 = e12 - e 22 - e32
a 22 = -e12 + e 22 - e32
a 33 = -e12 - e 22 + e32
81
Adding these equations yields
e12 + e 22 + e32 = a11 + a 22 + a 33 = trA = -1 (2.5.40)
which is a contradiction, so e0 = 0 cannot occur.
In both cases, choosing the +sign yields Euler parameter vector p + and the minus sign
yields p - ; i.e., p - = -p + . Thus, p + - p - = 2p + = 2 p + Tp + = 2 and the alternative values of
Euler parameters determined by the given orientation transformation matrix A are far apart, so
the solution is locally unique. This completes the proof of the theorem.
MATLAB Code 2.5 that is outlined in Section 2.5.6 and contained in Appendix 2.B,
implements the inverse Euler parameter mapping theorem.
The large distance between the alternate Euler parameter solutions p - and p + is important
in time-based analysis of the motion of mechanical systems, since Euler parameters are
determined on a finely spaced discrete time grid. Thus, it is not possible to jump from one of the
alternatives to the other when progressing from one grid point to the next. For practical purposes,
either selection of sign is acceptable to begin analysis on the time grid, and no singularity is
encountered (Stuelpnagel, 1964). As cited in (Stuelpnagel, 1964), Hopf proved in 1940 that a
five parameter representation of A is required to obtain a globally valid one-to-one
representation of orientation. While avoiding the foregoing alternative representation by
p - or p + , the extra cost of introducing five coordinates that must satisfy two constraints would
be oppressive.
2.5.5 Point Definition of an Orthogonal Reference Frame
One method of defining an orthogonal x ¢-y¢-z¢ reference frame, relative to the global
x-y-z frame, is to define point O as the origin of the x ¢-y¢-z¢ frame with vector r O , a point P on
the x ¢ axis with vector r P , and a point Q in the x ¢-y¢ plane that is not on the x ¢ axis with the
vector r Q , as shown in Fig. 2.5.3. The unit vector along the positive x ¢ axis, represented in the
( )
global frame, is f = 1 / r P - r O (r P - r O ) . Since r Q is in the x ¢-y¢ plane and r Q - r O ¹ 0 ,
f% (r Q - r O ) ¹ 0 is orthogonal to the x ¢-y¢ plane, hence it is along the positive z¢ axis. The unit
( )
vector along that axis is thus h = 1/ f% (r Q - r O ) f% (r Q - r O ) . Finally, the unit vector along the
% . The transformation matrix from the x ¢-y¢-z¢ frame to the x-y-z frame
positive y¢ axis is g = -fh
is, from Eq. (2.4.16), A = [f g h ] .
82
Figure 2.5.3 Points that Define an Orthogonal x ¢-y¢-z¢ Frame
MATALAB Code 2.5 in Appendix 2.B implements the point definition approach.
________________________________________________________________________
Example 2.5.2: As an example of using the foregoing definition of an orthogonal
x ¢-y¢-z¢ reference frame and the inverse Euler parameter mapping theorem to determine Euler
parameters that characterize its orientation, let r O = 0 , r P = [1 1 1] and r Q = [ -1 -1 0] .
T T
Then, f = (1/ 3) [1 1 1] , fr
% Q = (1/ 3) [1 -1 0]T , h = (1/ 2) [1 -1 0]T , and
T
% = (1/ 6) [ -1 -1 2]T . The orthogonal transformation matrix from the x ¢-y¢-z¢ frame to
g = -fh
the x-y-z frame is
é1/ 3 -1/ 6 1/ 2 ù
ê ú
A = ê1/ 3 -1/ 6 -1/ 2 ú
ê ú
êë1/ 3 2 / 6 0 úû
Since A is not symmetric and trA = 1 / 3 - 1/ 6 , Eq. (2.5.28) yields
e0 = ± (1/ 2) 1 / 3 - 1 / 6 + 1
é2 / 6 + 1 / 2 ù
æ 1 ö ê ú
e = ±ç ÷ ê1 / 2 -1 / 3 ú
ç ÷
è 2 1 / 3 - 1 / 6 + 1 ø ê 1 / 3 + 1/ 6 ú
êë úû
While it would be virtually impossible to use purely geometric analysis to specify the unit
vector u and rotational angle c in Euler’s theorem that define the orientation of the foregoing
x ¢-y¢-z¢ frame, analytical results using the inverse mapping theorem are easily obtained.
_______________________________________________________________________
83
2.5.6 Code 2.5, Point Definition of A and Solution for p with Given A
MATLAB Code 2.5 for point definition of an orientation transformation matrix and for
computation of Euler parameters for a given orientation transformation matrix is presented in
Fig. 2.5.4. Functions AT, atil, and partA are defined in the code.
1 %AA_Spatial_Orientation_Transformation_Matrix_and_Inverse for p
2
3 Part=[]; %Part=1, Define orthogonal tramsformation matrix
4 %Part=2, Compute p for given orthogonal tramsformation matrix A
5
6
7 if Part==1 %Define orthogonal tramsformation matrix
8
9 mode=[]; %mode=1, enter A; mode=2, point definition of A;
10 %mode=3, Euler'S Theorem
11
12 if mode==1
13 A=[,,;,,;,,];
14 end
15
16 if mode==2
17 %Define orthogonal transformation matrix A using point definition of
18 %Section 2.5.5.
19
20 rO=[;;]; %Enter vector rO to origin
21 rP=[;;]; %Enter vector rP to point P
22 rQ=[;;]; %Enter vector rQ to point Q
23
24 f=(1/norm(rP-rO))*(rP-rO);
25 h=(1/norm(atil(f)*(rQ-rO)))*atil(f)*(rQ-rO);
26 g=-atil(f)*h;
27
28 A=[f,g,h];
29
30 end
31
32 if mode==3 %Define orthogonal transformation matrix A using Euler's
33 %Theorem of Section 2.5.3.
34
35 %Unit vector u about which rotation angle chi brings x-y-z frame
36 %into coincidence with x'-y'-z' frame
37
38 ubar=[;;]; %Enter vector ubar about which rotation occurs,
39 %not necessarily normalized
40
41 chi=[]; %Enter angle chi of rotation
42
43 u=(1/norm(ubar))*ubar;
44 e0=cos(chi/2);
45 e=sin(chi/2)*u;
46 A=(e0^2-e'*e)*eye(3)+2*e*e'+e0*atil(e);
84
47 end
48
49 end
50
51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 if Part==2 %Given orthogonal A, compute Euler parameter vector p in
53 %Section 2.5.4 so A=A(p)
54
55 [a11,a12,a13,a21,a22,a23,a31,a32,a33] = partA(A);
56
C:\Users\echau\Docume...\AA_Spatial_Orientation_Transformation_Matrix_and_Inverse.m Page 2
57 if norm(A'-A)>0
58 trA=a11+a22+a33;
59 e0=0.5*sqrt(trA+1);
60 e=(1/(2*sqrt(trA+1)))*[a32-a23;a13-a31;a21-a12];
61 end
62
63 if norm(A'-A)==0
64 e0=1;
65 e=zeros(3,1);
66 end
67
68 p=[e0;e];
69
70 end
Figure 2.5.4 Code 2.5, Point Definition of A and Solution for p with Given A
Properties of the spatial orthogonal orientation transformation matrix lead to Euler’s theorem that
states that every orientation of a body-fixed reference frame, hence of a rigid body in space, can
be realized by a rotation about a unit vector in space that brings the global reference frame into
coincidence with the body-fixed frame. This leads to definition of a four-dimensional vector of
Euler parameters that characterize orientation of the body, but must satisfy a normalization
condition, thus three orientation degrees of freedom. The orientation transformation matrix is
written as a quadratic function of Euler parameters, a property that enables practical and efficient
computation in mechanical system kinematics and dynamics.
A classical set of three Euler angles that define orientation, but suffer an unavoidable singularity,
is defined in Appendix 2.A and contrasted with Euler parameters.
Relations are derived that represent the orientation transformation matrix in terms of vectors that
define three points in space. Conversely, relations are derived that determine locally unique Euler
parameters for a specified orthogonal transformation matrix. Both relations are implemented in
MATLAB computer code 2.5 in Appendix 2.B that is summarized in Section 2.5.6.
85
Key Formulas
χ χ
e0 º cos e º sin u (2.5.19)
2 2
( )
A = e 20 - e Te I + 2ee T + 2e 0e% (2.5.20)
ée 20 + e12 - e22 - e32 2 ( e1e 2 - e0 e3 ) 2 ( e1e3 + e 0e 2 ) ù

ê ú
A = ê 2 ( e1e 2 + e0e3 ) e -e +e -e
2
0
2
1
2
2
2
3 2 ( e 2e3 - e 0e1 ) ú (2.5.21)
ê 2 ( e1e3 - e0e 2 ) 2 ( e 2e3 + e0e1 ) e02 - e12 - e 22 + e32 úû
ë
p = [ e0 e3 ] = éëe 0
T
eT ùû
T
e1 e 2 p Tp = 1 p T p& = 0 (2.5.23) (2.5.24)
e0 = ± trA + 1 / 2 éa 32 - a 23 ù
b = ê a13 - a 31 ú
( )
(2.5.28)
e = ± 1/ (2 trA + 1) b ê ú
ëê a 21 - a12 ûú
86
2.6 Euler Parameter Identities and Derivatives
A number of identities and derivative relationships involving Euler parameters follow
from the quadratic form of terms in the orientation transformation matrix of Eq. (2.5.23), as
functions of Euler parameters. Relations based on these properties that are presented in this
section are critical in deriving and effectively implementing often complicated equations of
multibody kinematics and dynamics. Derivatives of kinematic constraint equations up to third
order arise in representing the geometry of nonlinear kinematic and kinetic equations that must
be incorporated into the equations of multibody dynamics and used in their numerical solution.
The identities and derivative relations presented in this section are used many times in
deriving equations of motion and, more important, in deriving and implementing numerical
methods for their computer solution. These results make practical what would otherwise be
hopelessly complex expressions required in computer programming. The reader is encouraged to
study the approach presented to obtain identities, which is based on differential calculus at the
sophomore level. To avoid the need to memorize results and for ease of reference, key formulas
derived are summarized at the end of the section. This is similar to the common practice in
differential calculus of tabulating derivatives of large numbers of transcendental functions in
book appendices, to avoid the need to memorize them.
MATLAB implementation of identities and derivatives are presented in Section 2.B.4 of
Appendix 2.B and in Code 2.6 in software that accompanies the text.
2.6.1 Euler Parameter Identities
Define 3 ´ 4 matrices that are linear functions of Euler parameters,
E ( p ) º [ -e e% + e0 I ] (2.6.1)
G ( p ) º [ -e -e% + e0 I] (2.6.2)
Multiplication and manipulation with vector identities of Section 2.1 verifies that
é -e T ù
E ( p ) G ( p ) = [ -e e% + e0 I ] ê ú = ee + ee
% % + 2e0e% + e20 I
T T
ëe + e0 I û
% (2.6.3)
= ee T + ee T - e TeI + 2e0e% + e20 I = ( e20 - e Te ) I + 2ee T + 2e0e%
Note that the right sides of Eqs. (2.5.22) and (2.6.3) are identical; i.e.,
A (p ) = E (p ) GT (p ) (2.6.4)
Two observations are important at this point. First, Eq. (2.6.4) was obtained without the
requirement that the vector p is normalized. Second, from Eqs. (2.6.3) and (2.6.4), it is observed
that all the entries in the matrix A ( p ) are quadratic in components of p, whereas each of the
factors on the right of Eq. (2.6.4) is a matrix whose entries are linear in p.
Some of the relations presented in Section 2.5 require that the vector of Euler parameters
satisfy the normalization condition p Tp = 1 . They should therefore not be used in iterative
numerical computations when p Tp ¹ 1 . A number of useful identities that involve Euler
parameters can be obtained without requiring satisfaction of the normalization condition; e.g.,
87
Eq. (2.6.4). These identities are valuable in developing relationships for derivatives of kinematic
expressions with respect to Euler parameters that are used in iterative computation, during which
the Euler parameter normalization condition may not be satisfied.
Carrying out the multiplications indicated shows that
ée ù
E ( p ) p = [ -e e% + e0 I ] ê 0 ú = -e0e + ee
% + e 0e = 0 (2.6.5)
e
ë û
where no Euler parameter normalization condition was required. Similarly,
G (p ) p = 0 (2.6.6)
Carrying out the multiplications indicated, for any p Î R 4 , yields
é -e T ù
E ( p ) E ( p ) = [ -e e% + e0 I ] ê
T
ú
ë -e% + e0 I û (2.6.7)
= ee T - ee
% % + e02 I = eeT - eeT + eTeI + e20 I = pTpI
Only if the Euler parameter normalization condition is satisfied is E(p)E T (p) = I . Similarly, for
any vector p Î R 4 ,
G(p)GT (p) = p TpI (2.6.8)
and only if the normalization condition is satisfied is G(p)GT (p) = I .

Carrying out the multiplications indicated yields
é e Te -e 0e T ù
ET ( p ) E ( p ) = [ -e e% + e0I ] [ -e e% + e0 I ] = ê
T
ú
ë -e0e -ee
% % + e20 I û
ée Te + e20 - e02 -e0 e T ù é p Tp 0 ù é e20 e0 e T ù
=ê =
ú ê ú-ê ú (2.6.9)
ë -e 0 e -ee T + e TeI + e20 I û ë 0 p TpI û ë e0e eeT û
= p TpI 4 - pp T
Only if the normalization constraint is satisfied is ET (p)E(p) = I 4 - ppT . Using the same form of
calculation,
G T ( p ) G ( p ) = p T pI 4 - pp T (2.6.10)
and only if the normalization constraint is satisfied is GT (p)G(p) = I4 - ppT .

A direct manipulation, using Eqs. (2.6.4), (2.6.7), (2.6.8), (2.6.9), and (2.6.10),
suppressing arguments, shows that
( ) ( )
2
AA T = EG T GE T = E p T pI - pp T E T = p T pEE T = p T p I (2.6.11)
and
88
( ) ( )
2
A T A = GE T EG T = G p T pI - pp T G T = p TpGG T = p T p I (2.6.12)
Thus, if p Tp = 1 , A is orthogonal; i.e., AA T = I and A T A = I , in which case, A T = A -1 .

Nevertheless, AA T = A T A , even if the normalization condition does not hold.
Carrying out the multiplications indicated, manipulating, and using Eq. (2.1.24),
ée 0 j ù
E ( pi ) p j = ëé -ei e% i + e0i I ùû ê ú = - e0 j ei + e% ie j + e0i e j
êë e j úû
(2.6.13)
é e0 ù
( )
= - -e 0i e j + e% jei + e 0 j ei = - éë -e j e% j + e0 j I ù ê i ú
û e
ë iû
which, for any p i and p j in R 4 is simply
E (p i )p j = - E (p j )p i (2.6.14)
A similar manipulation yields the identity

G (p i )p j = -G (p j )p i (2.6.15)
Direct manipulation, using Eqs. (2.1.24) and (2.1.32), yields

T
E(pi )G T (p j ) = éë -ei e% i + e 0i I ùû é -e j -e% j + e0 j I ù
ë û
= ei e j + e% ie% j + e 0 j e% i + e 0i e% j + e0i e0 j I
T
(2.6.16)
= e jei T + e% je% i + e 0i e% j + e0 j e% i + e0 j e0i I
Since the last two lines of Eq. (2.6.16) are identical, with the exception that indices i and j are
interchanged, for any p i and p j in R 4 ,
E(pi )G T (p j ) = E(p j )G T (pi ) (2.6.17)
Similarly, for any p i and p j in R 4 ,
G (p i )ET (p j ) = G (p j )ET (pi ) (2.6.18)
Define 4 ´ 4 matrices
é 0 -a T ù
R (a) º ê ú (2.6.19)
ëa a% û
é 0 -a T ù
T (a ) º ê ú (2.6.20)
ëa -a% û
where a is a 3×1 column vector. Direct manipulation yields
89
é 0 -a T ù é e 0 ù é -a T e ù
R (a) p = ê úê ú =ê ú
ëa a% û ë e û ëe 0a + ae % û
(2.6.21)
é -e Ta ù é -eT ù
ú a = [ -e e% + e0I ] a
T
=ê ú=ê
ëe 0a - ea
% û ë e0 I - e% û
where no use was made of Euler parameter normalization. From the definition of Eq. (2.6.1), this
is the identity
R(a)p = ET (p)a (2.6.22)
A similar manipulation yields the identity
T (a ) p = GT (p ) a (2.6.23)
2.6.2 Euler Parameter Derivatives
Let a¢ Î R 3 and p, g Î R 4 , where p and g are not normalized and a¢ and g do not
depend on p. Expanding the product indicated and taking the derivative with respect to p,
( A(p)a¢)p = éë( e0 2 - eT e ) a¢ + 2 ( eeT + e0e% ) a¢ùû p

= éë 2e0a¢ + 2ea
% ¢ -2ea
% % ¢ + 2ea¢T - 2e0a% ¢ùû (2.6.24)
= 2 éë( e 0I + e% ) a¢ ea¢T - ( e0 I + e% ) a% ¢ ùû
( ) (
where, using Eqs. (2.1.24) and (2.1.27), e Tea¢ = a¢eT e = a¢ eT e = 2a¢eT and
e
) e
( ) e
( ee a¢ ) = {e ( a¢ e )}
T
e
T
e
T
( T
)
= ea¢ + a¢ e I . Using Eq. (2.1.29), -2a¢e + 2eT a¢I = -2ea
% % ¢ . Defining T
B ( p,a¢ ) º 2 éë( e0 I + e% ) a¢ ea¢T - ( e0 I + e% ) a% ¢ùû = 2E ( p ) T ( a¢ ) (2.6.25)
where the second equality on the right is verified by direct manipulation, the following derivative
identity that does not require normalization of p is obtained:
( A (p)a¢ )p = B(p, a¢) (2.6.26)
Expanding the product indicated, using Eqs. (2.1.24) and (2.1.32),
( ) (
B ( pi ,a¢ ) p j =2 é e0i I + e% i a¢ ei a¢T - e 0i I + e% i a% ¢ ù p j
ë û )
( )
= 2 é e0i e 0 j I + e0 j e% i a¢ + ei a¢Te j + e 0i e% ja¢ + e% i e% ja¢ ù
ë û
= 2 é( e e I + e e% ) a¢ + e e a¢ + e e% a¢ + e% e% a¢ ù
T
(2.6.27)
ë 0 j 0i 0j i i j
û
0i j i j
= 2 é( e e I + e e% ) a¢ + e e a¢ + e e% a¢ + e% e% a¢ ù
T
ë 0 j 0i 0i j j i
û
0j i j i
= 2 é( e e I + e e% ) a¢ + e a¢ e + e e% a¢ + e% e% a¢ ù
T
ë 0 j 0i 0i j j i
û
0j i j i
90
Since the second and last expressions on the right are identical, with the exception that the roles
of i and j have been reversed, this yields the identity
B (p i , a¢)p j = B (p j , a¢)p i (2.6.28)
which does not require p i or p j in R 4 to be normalized. Using the approach employed to obtain
Eq. (2.6.26),
( A (p ) a ) = (( e
T
p
2
0 - e Te ) a + 2ee Ta - 2e0ea
% ) p
= éë 2e0a - 2ea
% -2aeT + 2a TeI + 2ea T + 2e0a% ùû
(2.6.29)
= 2 éë( e0 I - e% ) a +eaT - ea
% % + e0a% ùû
= 2 éë( e0 I - e% ) a +eaT + ( e0 I - e% ) a% ùû
and a derivative relation is obtained, with no requirement for normalization of p,
(A T
(p )a ) = C(p, a )
p
(2.6.30)
where
C ( p,a ) º 2 éë( e0 I - e% ) a ea T + ( e 0 I - e% ) a% ùû = 2G (p) R (a) (2.6.31)
the second equality being verified by direct manipulation. Using the same approach employed to
derive Eq. (2.6.28), the following identity, not requiring p i or p j in R 4 to be normalized, is
obtained:
C(p i , a )p j = C(p j , a )p i (2.6.32)
Equation (2.6.28) provides an easy result for the derivative
( B ( p ,a¢) p )
i j p
i
(
= B ( p j ,a¢ ) pi ) pi
= B ( p j ,a¢) (2.6.33)
without Euler parameter normalization conditions. Similarly, Eq. (2.6.32) yields
( C ( p ,a ) p )
i j p
i
(
= C ( p j ,a ) pi ) pi
= C ( p j ,a ) (2.6.34)
without Euler parameter normalization conditions.

Expanding the products indicated, using Eqs. (2.1.22), (2.1.24), (2.1.32), and (2.1.27),
B T ( p,a¢ ) b = 2 ëé( e 0 I + e% ) a¢ ea¢T - ( e0I + e% ) a% ¢ ùû b

T
é e a¢T b - a¢T eb % ù é e0a¢T b + a¢T be % ù

= 2 ê T0 ú = 2 ê ú (2.6.35)
%
ëa¢e b + e 0a% ¢b - a% ¢eb
% û ë e0a% ¢b + a¢b e + a% ¢be û
T
éa¢Tb a¢T b% ù éa¢T b a¢Tb% ù

= 2ê ú p = 2 ê úp
%
ë a% ¢b a¢b + a% ¢b û ë a% ¢b a¢b + ba¢ - a¢ bI û
T T T T
with no normalization condition on p. This yields the identity
91
BT (p, a¢)b = K (a¢, b)p (2.6.36)
where
éa¢T b a¢T b% ù
K ( a¢,b ) º 2 ê ú (2.6.37)
ë a% ¢b a¢b + ba¢ - a¢ bI û
T T T
A direct manipulation shows that K ( a ¢,b ) is symmetric; i.e., for any values of a¢ and b,
K T ( a¢,b ) º ( K ( a¢,b ) ) = K ( a¢,b ) . Use of Eq. (2.6.36) yields the derivative relation
T
( B ( p,a¢) g ) = ( K ( a¢,g ) p )
T
p p
= K ( a¢,g ) (2.6.38)
Manipulations identical to those used in obtaining Eq. (2.6.35) yield
CT ( p,a ) b = 2 ëé( e0 I - e% ) a ea T + ( e0 I - e% ) a% ûù b
T
é e aTb + a Teb% ù %
é e0a Tb - a Tbe ù
= 2ê T0 ú = 2 ê ú (2.6.39)
ëae b - e0ab
% - aeb%% û ëab e - e0ab
T
%% û
% + abe
é aTb -a Tb% ù é aTb -a Tb% ù
= 2ê ú p = 2 ê úp
ë -ab
% ab + ab
T
%%û ë -ab
% ab + ba - a bI û
T T T
where no normalization condition on p is required. This yields the identity

CT (p, a)b = L(a, b)p (2.6.40)
where
é aT b -a Tb% ù
L ( a,b ) º 2 ê ú (2.6.41)
ë -ab
% ab T + ba T - a TbI û
and L ( a¢,b ) is symmetric. Equation (2.6.40) yields the derivative relation
( C ( p ,a ) b ) = ( L ( a,b ) p )
T
p p
= L ( a,b ) (2.6.42)
Using the identities of Eqs. (2.6.14) and (2.6.15), the following derivatives are obtained,
without requiring that p or γ in R 4 be normalized:
( E(p) g )p = -E( g ) (2.6.43)
( G (p) g )p = -G ( g ) (2.6.44)
Expanding the product indicated, using Eqs. (2.1.23) and (2.1.25),

é -e T a ù é 0 -a T ù
E ( p )a = [ - e e + e 0 I ] a = ê
T
T
% = p
% + e0a ûú êëa a% úû
(2.6.45)
ë -ea
Using the definition of Eq. (2.6.19), this is the identity
ET (p)a = R (a)p (2.6.46)
92
which, with no normalization condition, yields the derivative relationship
(E T
(p )a ) = R (a )
p
(2.6.47)
Using the same manipulations yields analogous identities and derivative relationships,
é -e T a ù é 0 -a T ù
G T (p )a = [ -e -e% + e0 I ] a = ê
T
ú=ê úp (2.6.48)
ë %
ea + e a
0 û ë a - %
a û
G T (p ) a = T (a ) p (2.6.49)
(G (p) a)
T
p
= T (a) (2.6.50)
where no normalization condition on the vector p is required.

It is of value for manipulations to be carried out in kinematic and dynamic analysis to
note that the operators B(p, a¢), C(p, a ), K(a¢, b), and L(a, b) of Eqs. (2.6.25), (2.6.31), (2.6.37),
and (2.6.41) are bilinear in their arguments. That is, with either argument fixed, they are linear in
the other argument.
2.6.3 Time Derivatives and Variations of Euler Parameters
Since orientation of a body in space varies with time, the orientation transformation
matrix is a function of time, A = A ( t ) . Thus, the Euler parameter vector p that defines A ( t )
must be a function of time; i.e., A ( t ) = A ( p ( t ) ) . Since the Euler parameter normalization
constraint must hold at all orientations; i.e. for all t,
pT ( t ) p ( t ) = 1 (2.6.51)
Equation (2.6.51) may be differentiated with respect to time to obtain p Tp& + p& Tp = 2p Tp& = 0 ,
equivalently,
pT p& = 0 (2.6.52)
It is important to recall that this condition is only satisfied if the Euler parameter normalization
condition is imposed for all time.
( ) ( )
Using the identity E ( pi ) G T p j = E p j G T ( pi ) of (2.6.17) with p i = p& and p j = p ,
E ( p& ) G T ( p ) = E ( p ) G T ( p& ) (2.6.53)
Differentiating Eq. (2.6.4) in the form A ( t ) = E ( p(t) ) G T ( p(t) ) with respect to time, using the fact
that E(p) and G(p) are linear in p, and using (2.6.53),
& = EG
A & T (p) + E(p)G & T = E ( p& ) G T (p) + E ( p ) GT ( p& )
(2.6.54)
= 2E ( p ) GT ( p& ) = 2E(p)G & T = 2EG & T (p)
where E & = G(p& ) . This result holds even if there is no Euler parameter
& = E(p& ) and G
normalization.
93
The product E(p)p& may be expanded as
é e& ù
E(p)p& = [ -e e% + e0 I ] ê 0 ú = -e& 0e + ee
% & + e0e& (2.6.55)
ë e& û
Applying the tilde operator to both sides of this equation and manipulating using Eqs. (2.1.32),
(2.1.30), and the second of Eqs. (2.5.26),
²& = -²
E(p)p e& 0e + ee ° + e e&% = -e& e% + ee
%& + e0e& = -e& 0e% + ee
%& %&% - ee
% + e e&%
&%
0 0 0
%&% - ee& + e& eI + e0e&% = -e& 0e% + ee

= -e& 0e% + ee T T
%&% - ee& - e0e& 0I + e0e&%
T
= -E(p)ET (p)
&
where e Te& = -e0e& 0 has been used from p Tp& = 0 . Thus, this equation holds only if p Tp& = 0 . Taking
the transpose of both sides of this equation yields
²& = E(p)E
E(p)p & T
& T (p) = EE (2.6.56)
which only holds only if p Tp& = 0 .

From Eqs. (2.4.48) and (2.4.49), using Eqs. (2.6.4),
(2.6.54), (2.6.9), (2.6.10), (2.6.5), and (2.6.6) and p Tp = 1 ,
w & T (p) = 2 EG
% = AA & T (p)G(p)E T (p) = 2 E ( )
& I - pp T E T (p) = 2 EE
& T (p)
(2.6.57)
& = 2G(p)E T (p)E(p)G
% ¢ = A T (p)A
w & T = 2G(p) I - pp T G (
& T = 2G(p)G
&T )
Equation (2.6.56) yields
w ²&
% = 2E(p)p (2.6.58)
equivalently, the desired relation
w = 2E(p)p& (2.6.59)
Multiplying both sides of Eq. (2.6.59) on the left by ET (p) yields
(
E T (p)w = 2 ET (p)E(p)p& = 2 p TpI - pp T p& ) (2.6.60)
Providing p Tp = 1 and pTp& = 0 , this yields the inverse relationship

1
p& = ET (p)w (2.6.61)
2
Thus, even though the matrix E(p) is not square, it behaves much like an orthogonal matrix in
Eqs. (2.6.59) and (2.6.61).
From Eq. (2.6.59),
w¢ = A T (p)w = 2G(p)E T (p)E(p)p& = 2G(p) I - pp T p& ( ) (2.6.62)
94
Since G(p)p = 0 ,
w¢ = 2G(p)p& (2.6.63)
Multiplying both sides by GT (p) , G Tw¢ = 2G T (p)G(p)p& = 2 ( I - pp T ) p& and, provided p Tp = 1 ,

the inverse relation follows, just as in Eq. (2.6.61); i.e.,
1
p& = G T (p)w¢ (2.6.64)
2
It is important to note that Eqs. (2.6.57) through (2.6.64) hold only if p Tp = 1 for all time.
2.6.4 Euler Parameter Differentials and Infinitesimal Rotation
Repeating the manipulations of Eqs. (2.6.52) through (2.6.64), replacing derivative d/dt
with the differential δ of Section 2.4.6 and suppressing p arguments,
pTδp = 0 (2.6.65)
δA = 2E(p)δGT = 2δEGT (p) (2.6.66)

²
E(p)δp = -E(p)δE T (2.6.67)
δp = 2 E(p)δp
(2.6.68)
δp ¢ = 2G(p)δp
δp = (1/2)ET (p)δp = (1/2)GT (p)δp¢ (2.6.69)

Each of Eqs. (2.6.65) through (2.6.69) is valid only if the Euler parameter normalization
constraint is satisfied and p T δp = 0 .
2.6.5 Nonintegrability of Angular Velocity
To determine whether angular velocity can be obtained as the time derivative of some
function of Euler parameters, expanding Eq. (2.6.59) and using Eq. (2.6.2) yields
ée& ù
ω = 2E ( p ) p& = 2 [ -e e% + e0 I ] ê 0 ú = 2 ( -ee& 0 + ee
% & + e0e& ) (2.6.70)
ë e& û
The first component of Eq. (2.6.70) is
3
ω x = 2 ( -e1e& 0 + e0 e& 1 - e3e& 2 + e 2 e& 3 ) º 2 å a i e& i (2.6.71)
i =0
The Exact Differential Theorem of Section 2.2 can be used to determine whether the differential
form on the right of Eq. (2.6.71) is exact. It is the time derivative of a function of p if and only if
¶a i ¶a j
= for all i and j in the range 0 to 3. With i=0 and j=1,
¶e j ¶e i
¶a 0 ¶ ( -2e1 ) ¶ ( 2e0 ) ¶a1

= = -2 ¹ 2 = = (2.6.72)
¶e1 ¶e1 ¶e 0 ¶e 0
95
Thus, the differential form of Eq. (2.6.71); i.e., the x component of angular velocity, is not
exact, so it is not the time derivative of any function of Euler parameters. This shows that
angular velocity cannot be integrated to obtain a function of Euler parameters. Similarly, while
dπ is an infinitesimal rotation, it is not the differential of any function of Euler parameters.
2.6.6 Angular Acceleration as a Function of Euler Parameter Derivatives
Differentiating both sides of
a = A ( p ) a¢ (2.6.73)
where a¢ is constant and using the identity of Eq. (2.6.26),

a& = ( A ( p ) a¢ )p p& = B ( p,a¢ ) p& (2.6.74)
Taking the time derivative of both sides of Eq. (2.6.74) and using the identity of Eq. (2.6.28),
&& + ( B ( p, a¢ ) p& ) p&
a = B ( p, a¢ ) p
&&
p
(2.6.75)
= B ( p,a¢ ) p
&& + B ( p,a
& ¢ ) p&
As expected, acceleration is a linear function of Euler parameter second derivatives, with

coefficients that depend on Euler parameters, and it has quadratic terms in Euler parameter first
derivatives.
Relations for angular acceleration as a function of Euler parameter second derivatives
may be obtained from Eqs. (2.6.59) and (2.6.63),
w && + 2E ( p& ) p& = 2E(p)p
& = 2E(p)p && (2.6.76)
where Eq. (2.6.5) has been used with p replaced by p& , and
w && + 2G ( p& ) p& = 2G(p)p

& ¢ = 2G(p)p && (2.6.77)
where Eq. (2.6.6) has been used with p replaced by p& . These relations hold only if p is normalized,
for all time.
&& as a function of w
In order to determine p & ¢ , Eqs. (2.6.76) and (2.6.77) may be
& and w
multiplied on the left by E and G , suppressing arguments, to obtain
T T
ET w
& = 2ET Ep ( )
&& = 2 I - ppT p
&& = 2p
&& - 2ppTp
&& (2.6.78)
G wT
&& = 2 ( I - pp ) p
& ¢ = 2G Gp T
&& = 2p
T
&& - 2pp p
&& T
(2.6.79)
Differentiation of the identity p Tp& = 0 of Eq. (2.6.52) and using &p = (1/2)G T ω¢ of Eq. (2.6.64)
and GG T = ppT I3 of Eq. (2.6.8),
( )
T
p = -p& T p& = -(1/4)ω¢T ω¢(p T p ) = -(1/4) A T w A T w(p T p ) = -(1/4)ωT ω(p Tp )
p T &&
Substituting this result in Eqs. (2.6.78) and (2.6.79) and enforcing p Tp = 1 yields
p = 12 E T w
&& & - 14 wT wp (2.6.80)
96
&& & ¢ - 14 w¢T w¢p
p = 12 G T w (2.6.81)
Thus, there are explicit transformations from angular acceleration to Euler parameter second
derivatives. However, they involve terms that are quadratic in Euler parameter first derivatives
2.6.7 Special Identities
Identities that are helpful in analysis of the equations of dynamics in specific applications
are derived. Forming products from Eqs. (2.6.1) and (2.6.2) and from Eqs. (2.6.19) and (2.6.20)
and manipulating using vector identities from Section 2.1 to move factors involving a to the left,
yields
é 0 aT ù
G ( p ) TT ( a ) = [ -e -e% + e0 I ] ê ú
ë -a a% û
= éë -ae0 - ae % % ùû
% -eaT + a% e0 - ea
= éë -ae0 - ae % % + ea T - ae T + e0a% ùû
% -eaT - ae (2.6.82)
= a% ëé -e ( e0 I - e% ) ùû - a éëe0 e T ùû
% ( p ) - ap T
= aG
% ( p ) - ap T
E(p)R T (a ) = -aE
which do not require normalization of p.
A helpful identity in applications is obtained by carrying out the following manipulations,
using Eqs. (2.6.25), (2.6.82), (2.6.4), (2.6.5), (2.6.31), and (2.6.6), with no Euler parameter
normalization condition:
(
G(p)BT (p,a¢) = 2G(p)TT (a¢)E T (p) = 2 a% ¢G(p) - a¢pT E T (p) )
( ) - 2a¢ ( E(p)p )
T
= 2a% ¢ E(p)GT (p)
T
= 2a% ¢A T (p)
(2.6.83)
(
E(p)C (p,a¢) = 2E(p)R T (a¢)GT (p) = 2 -a% ¢E(p) - a¢pT G T (p)
T
)
= -2a% ¢E(p)G T (p) - 2a¢ ( G(p)p )
T
= -2a% ¢A(p)
T
A helpful relation in solving equations of motion involves the matrix éëp G T ( p ) ùû ; i.e.,
T -1
é pT ù é pT ù
ê ú = ê ú (2.6.84)
ë G (p ) û ë G (p ) û
which holds, based on Eq. (2.6.10), as long as p is normalized.
2.6.8 Derivatives with Respect to Model Parameters
In modeling systems for applications, parameter vectors that define geometry of the
model are occasionally used as generalized coordinates. Derivatives with respect to these
parameters are thus needed. For example, forming the products indicated and manipulating,
97
( )
B ( p i ,a¢i ) p j = 2 é e0i I + e% i a¢i ei a¢i T - e 0i I + e% i a% ¢i ù p j
ë û ( )
{( ) (
= 2 e0i e0 j I + e 0 j e% i a¢i + ei eTj a¢i + e0i I + e% i e% ja¢i ) }
Differentiating with respect to a¢i yields
( B ( p , a¢ ) p )
i i j a¢
i
{( )
= 2 e0i e 0j I + e 0 j e% i + ei eTj + e 0i I + e% i e% j( ) }
Carrying out expansion of products indicated yields the relation
( B ( p , a¢ ) p )
i i j a¢
i
{ }
= 2 E ( p i ) G T ( p j ) º M ( p i ,p j ) = M ( p j ,p i ) (2.6.85)
where Eqs. (2.6.17) and (2.6.85) are used. Similarly, forming the product
C ( p i , ai ) p j = 2 é e0i I - e% i ai
ë ( ) ( )
ei ai T + e0i I - e% i a% i ù p j
û
and differentiating with respect to a¢i , an identical manipulation to that above yields
( C ( p ,a ) p )
i i j a
i
{ }
= 2 G ( p i ) E T ( p j ) º N ( p i ,p j ) = N ( p j ,p i ) (2.6.86)
Differentiation of Eqs. (2.6.85) and (2.6.86), using Eq. (2.6.43), with no normalization
requirement, yields
( M ( p,γ ) b ) = 2 ( E ( p ) G ( g ) b ) = -2E ( G ( g ) b ) º Z(g ,b)

p
T
p
T
(2.6.87)
( N ( p,γ ) b ) = 2 ( G ( p ) E ( g ) b ) = -2G ( E ( g ) b ) º Y( g, b)
p
T
p
T
Finally, forming the product indicated, differentiating, and manipulating,
é a¢T ( e0 I - e% ) b ù é b T ( e0 I + e% ) a¢ ù
( B (p,a¢) b )
T
= 2ê T = ê ú
ú
( )
2
a¢
ë a ¢e b + %
a ¢ ( e 0 I - %
e ) b û a¢ ê
ë
e T
ba ¢ - e 0
% ¢ + eb
ba ± a ¢ú
%
û a¢
é b T ( e0 I + e% ) ù é b T ( e0 I + e% ) ù
= 2ê = ê T % ú
% ú
2 (2.6.88)
ëbe - b ( e0 I + e% ) û
%
ë - e0 b + be - be% û
T
= CT ( p , b )
A similar manipulation shows that
( C ( p, b ) a ¢ )
T
b
= B T ( p, a ¢ ) (2.6.89)
None of the relations of this section require normalization of p.
98
A number of algebraic identities follow from quadratic dependence of the orientation
transformation matrix on Euler parameters. They are extended to time derivatives and
differentials that define angular velocity, angular acceleration, and virtual rotations, serving as
the basis for a practical theory and computational method for characterizing kinematics and
dynamics of mechanical systems.
Derivative relations obtained using basic differential calculus lead to a family of differential
operators that form the foundation for practical computer formulation and solution of equations
of kinematics and dynamics. Without the identities, detailed manipulations that were involved
in their derivation would be repeated ad nauseum; i.e., to a sickening degree that would be
intractable if an ad-hoc differential calculus approach were adopted. Identities relating to
system design parameters that have been found helpful in applications are also provided. Key
formulas from this and preceding sections are provided as a central reference in Appendix 2.C.
MATLAB computer code that implements identities and differential operators is provided as
Code 2.6 in Appendix 2.B.
Key Formulas (Only formulas with bold italic equation numbers require p T p = 1 )
E ( p ) º [ -e e% + e0 I ] G ( p ) º [ -e -e% + e0 I] (2.6.1) (2.6.2)

A (p ) = E (p ) G (p )
T
E(p)p = 0 G(p)p = 0 (2.6.4) (2.6.5) (2.6.6)
E(p)ET (p) = pTpI G(p)GT (p) = p TpI (2.6.7) (2.6.8)
E T (p)E(p) = p TpI 4 - pp T GT (p)G(p) = pTpI4 - ppT (2.6.9) (2.6.10)

E (p i )p j = - E (p j )p i G (p i )p j = -G (p j )p i (2.6.14) (2.6.15)
é0 -aT ù é0 -aT ù
R(a) º ê ú T ( a ) º ê ú (2.6.19) (2.6.20)
ëa a% û ëa -a% û
B ( p,a¢ ) º 2 éë( e0 I + e% ) a¢ ea¢T - ( e0 I + e% ) a% ¢ùû = 2E ( p ) T ( a¢ ) (2.6.25)
C ( p,a ) º 2 éë( e0 I - e% ) a ea T + ( e 0 I - e% ) a% ùû = 2G (p) R (a) (2.6.31)
( A (p)a¢ )p = B(p, a¢) (A T

(p )a ) = C(p, a )
p
(2.6.26) (2.6.30)
B (p i , a¢)p j = B (p j , a¢)p i C(p i , a )p j = C(p j , a )p i (2.6.28) (2.6.32)
éa¢T b a¢T b% ù
K ( a¢,b ) º 2 ê ú ( B ( p,a¢) g )
T
= K ( a¢,g ) (2.6.37) (2.6.38)
ë a% ¢b a¢b + ba¢ - a¢ bI û
T T T p
é aT b -aTb% ù
L ( a,b ) º 2 ê ú ( C ( p ,a ) b )
T
= L ( a,b ) (2.6.41) (2.6.42)
ë -ab
% ab + ba - a bI û
T T T p
( E(p) g )p = -E( g ) ( G (p) g )p = -G ( g ) (2.6.43) (2.6.44)
(E T
(p )a ) = R (a )
p
( G T ( p ) a )p = T ( a ) (2.6.47) (2.6.50)
pTp& = 0 & = 2E(p)G

A & T = 2EG
& T (p) (2.6.52) (2.6.54)
99
w = 2E(p)p& w¢ = 2G(p)p& (2.6.59) (2.6.63)
w
& = 2E(p)p
&& & ¢ = 2G(p)p
w && (2.6.76) (2.6.77)
1 1
p& = ET (p)w p& = GT (p)w¢ (2.6.61) (2.6.64)
2 2
p = 12 E T w - 14 wT wp
&& && & ¢ - 14 w¢T w¢p
p = 12 G T w (2.6.80) (2.6.81)
pTδp = 0 δA = 2E(p)δGT = 2δEGT (p) (2.6.65) (2.6.66)

δp = 2E(p)δp δp ¢ = 2G(p)δp (2.6.68)
1 1
δp = ET (p)δp δp = GT (p)δp¢ (2.6.69)
2 2
G ( p ) T T ( a ) = aG
% ( p ) - ap T % ( p ) - ap T
E(p)R T (a) = -aE (2.6.82)
G(p)BT (p,a¢) = 2a% ¢AT (p) E(p)CT (p,a¢) = -2a% ¢A(p) (2.6.79)
( B ( p ,a ¢ ) p )
i i {
j a¢
i
( )} º M (p ,p ) = M (p ,p )
= 2 E (pi ) GT p j i j j i (2.6.85)
(C ( p ,a ) p ) = 2{G ( p ) E ( p )} º N (p ,p ) = N (p ,p )
i i j a
i
i
T
j i j j i (2.6.86)
( M ( p,γ ) b ) = 2 ( E ( p ) G ( g ) b ) = -2E ( G ( g ) b ) º Z(g ,b)

p
T
p
T
(2.6.87)
( N ( p,γ ) b ) = 2 ( G ( p ) E ( g ) b ) = -2G ( E ( g ) b ) º Y( g, b)
p
T
p
T
( B ( p, a ¢ ) b )
T
a¢
= CT ( p,b ) ( C (p,b ) a¢)
T
b
= B ( p,a¢)
T
(2.6.88) (2.6.89)
100
Appendix 2.A Euler Angle Orientation Coordinates
A classical set of orientation parameters for bodies in space, Euler angles, is presented in
this section. Three sequential rotations are used to determine orientation. The concept of Euler
angles is an extension of the process in Section 2.3 of using a single rotation angle to define
orientation in the plane. Euler angles, however, are shown to (1) suffer from having a singular
orientation that must be avoided in practice and (2) involve extensive transcendental function
expansion in derivative expressions, which greatly complicates kinematic and dynamic
computations. While the multibody dynamics community is turning to Euler parameters treated
in Sections 2.5 and 2.6, there is adequate historical experience with Euler angles to justify their
introduction.
To assist in the transition from classical Euler angles for multibody kinematics and
dynamics to a modern setting, consider several points from the landmark paper by Stuelpnagel
(1964). He observes that in 1776, Euler showed that the orientation transformation rotation group
is three dimensional. Over the intervening two- and one-half centuries, many attempts have been
made to find a set of three orientation generalized coordinates that uniquely define orientation of
bodies in space and is well suited to equation formulation and solution. A motivation in this
search was the belief that a minimal set of three coordinates is superior to a redundant set; i.e.,
four or more orientation parameters that are subject to constraints.
In 1940, Hopf (1940) showed that five is the minimum number of coordinates to
represent the orientation transformation group in a globally one-to-one manner. Stuelpnagel
showed, however, that the 4-dimensional "quaternion method" (Goldstein, 1980), which is a
precursor of Euler parameters presented in Sections 2.5 and 2.6, is a one-to-two parameterization
that practically avoids singularities, as is shown in Section 2.5. He furthermore proved that it is
impossible to have a set of three orientation coordinates that avoids singularity. Nevertheless,
many in the mechanical system dynamics community have continued the search for three utopian
parameters, while using Euler angles and their numerous variants well into the 20th century.
Acknowledging the singularity at q = 0 that is demonstrated herein, users of Euler angles
monitor this critical condition and, as it is approached, halt simulation, define a new reference
frame, restructure the multibody system model, and restart the simulation.
Late in the twentieth century, the use of four-dimensional Euler parameters to avoid
singularities became more common. It has become increasingly clear that the algebraic
properties of Euler parameters, based on the quadratic character of entries in the orientation
transformation matrix, rather than products of transcendental functions of Euler angles, greatly
simplifies expressions for derivatives of functions that are involved in kinematics and dynamics.
These and related considerations associated with computer formulation and solution of high
dimensional multibody dynamics models show that the penalty associated with four dimensional
Euler parameters is more than offset by their analytical simplicity and computational efficiency.
2.A.1 Definition of Euler Angles
The sequence of three rotations shown in Fig. 2.A.1 defines an orientation of the
x ¢ - y¢ - z ¢ frame, relative to the x - y - z reference frame. With the x ¢ - y¢ - z ¢ frame initially
coincident with the x - y - z reference frame, counterclockwise rotation by angle f about the
positive z axis is implemented to form the x ¢¢¢ - y¢¢¢ - z¢¢¢ frame shown in Fig. 2.A.1. Next,
counterclockwise rotation by angle q about the positive x ¢¢¢ axis is implemented, to obtain the
101
x ¢¢ - y¢¢ - z ¢¢ frame. Finally, counterclockwise rotation by angle y about the positive z¢¢ axis is
implemented to obtain the desired x ¢ - y¢ - z¢ frame, in its general orientation shown in Fig. 2.A.1.
Figure 2.A.1 Euler Angle Definition

To develop analytical expressions for elements of the orientation transformation matrix A
as functions of Euler angles, each of the rotations is considered separately. The first rotation is
shown in Fig. 2.A.2. Since this is precisely the rotation defined in Section 2.3, the results of
calculations carried out there; i.e., Eq. (2.3.27), yield the transformation
é cosf -sinf 0 ù
s = ê sinf cosf 0 ú s¢¢¢ º Bs¢¢¢
ê ú (2.A.1)
êë 0 0 1 úû
Figure 2.A.2 First Euler Angle

The second Euler angle rotation is shown in Fig. 2.A.3. Much as in the case of the first
rotation treated in Section 2.3, the direction cosine transformation is
102
é1 0 0 ù
s¢¢¢ = 0 cosq -sinq ú s¢¢ º Cs¢¢
ê (2.A.2)
ê ú
ëê 0 sinq cosq ûú
Finally, the third Euler angle rotation is shown in Fig, 2.A.4. Since it is identical in form to the
first, the direction cosine transformation relation is obtained as
é cos y -siny 0 ù
s¢¢ = ê siny cos y 0 ú s¢ º Ds¢ (2.A.3)
ê ú
êë 0 0 1 úû
Figure 2.A.3 Second Euler Angle Figure 2.A.4 Third Euler Angle
Substituting the result of Eq. (2.A.3) into Eq. (2.A.2) and subsequently into Eq.
(2.A.1), the transformation from the x ¢ - y¢ - z¢ frame to the x - y - z reference frame is
s = BCDs ¢ º As ¢ (2.A.4)
so
A = BCD (2.A.5)
Substituting for B, C, and D from Eqs. (2.A.1) through (2.A.3) into Eq. (2.A.5) and
carrying out the multiplications,
é cos ycosf - cosqsinfsiny -sinycosf - cosqsinfcos y sinqsinf ù
A = ê cosysinf + cosqcosfsin y -sinysinf + cosqcosfcos y -sinqcosf ú (2.A.6)
ê ú
êë sinqsiny sinqcosy cosq úû
Equation (2.A.4) provides the desired Euler angle orientation transformation, where A is
defined by Eq. (2.A.6).
2.A.2 Euler Angle Singularity
In order to resolve the issue of singularity of the transformation from A to Euler angles,
consider the transformation sequence shown in Fig. 2.A.5 with q = 0 . In this special case, the
first and third rotation angles, f and y , are about the same axis. Therefore, for any a , f can
be replaced by f + a and y can be replaced by y - a to yield the same orientation
103
transformation matrix. Thus, the Euler angles are not uniquely determined, even locally, by the
orientation transformation matrix when q = 0 . The transformation from A to Euler angles is thus
singular when q = 0 or any multiple of p . This is the Euler angle singularity.
z, z¢¢¢, z¢¢, z¢
y¢
y y¢¢¢, y¢¢
f
y
f y x¢
x
x ¢¢¢, x ¢¢
Figure 2.A.5 Euler Angle Rotations With θ = 0

Since angular velocity and acceleration are of concern in kinematic and dynamic
analysis, it is desired to obtain a relationship between the time derivative of Euler angles and
angular velocity. First, the time derivative of A is obtained by taking the time derivative of both
sides of Eq. (2.A.5), using the product rule of differentiation,
A& = BCD
& & + BCD
+ BCD & (2.A.7)
Equation (2.4.49), using Eqs. (2.A.5) and (2.A.7), yields
&
% ¢ = AT A
ω
&
= DTCT B T BCD + BCD(
& + BCD
& ) (2.A.8)
&
= DTCT B T BCD & + DT D
+ DT CT CD &
where the fact that B, C, and D are orthogonal matrices has been used. Beginning with the third
term on the right of Eq. (2.A.8), direct calculation yields
é cos y siny 0 ù é -siny -cos y 0 ù é 0 -1 0 ù
D D = -siny cos y 0 cos y -siny 0 y& = ê1 0 0 ú y&
T & ê ú ê ú (2.A.9)
ê úê ú ê ú
êë 0 0 1 úû êë 0 0 0 úû êë 0 0 0 úû
Similarly,
é0 0 0 ù é 0 0 -siny ù
T ê ú ê
T &
D C CD = D 0 0 -1 Dq = 0
T & 0 -cos y ú q& (2.A.10)
ê ú ê ú
êë 0 1 0 úû êësiny cos y 0 úû
Finally, carrying out a somewhat longer expansion yields
104
é 0 -cosq cos ysinq ù
T &
D C B BCD =
T T ê cosq 0 -sinqsiny ú f& (2.A.11)
ê ú
êë -cos ysinq sinqsiny 0 úû
%¢,
Substituting from Eqs. (2.A.9) to (2.A.11) into Eq. (2.A.8) yields a 3 ´ 3 matrix for ω
é 0 -cosq cos ysinq ù é 0 0 -siny ù é 0 -1 0 ù
%¢=
ω ê cosq 0 ú ê
-sinqsiny f& + 0 0 -cos y q& + ê 1 0 0 ú y
ú &
ê ú ê ú ê ú
êë -cos ysinq sinqcos y 0 úû êësiny cos y 0 úû êë 0 0 0 úû
(2.A.12)
Since the matrix on the right of Eq. (2.A.12) is skew symmetric, components of angular velocity
ω¢ are identified, using Eq. (2.1.22) as
é0 ù é cos y ù é sinysinq ù
ê
ω¢ = 0 y ú & + -siny q + êcos ysinqú f&
ê ú & (2.A.13)
ê ú ê ú ê ú
êë1 úû êë 0 úû êë cosq úû
To determine f& , q& , and y& , once ω¢ is known, Eq. (2.A.13) may be written as
the matrix equation
é sinysinq cos y 0 ù é f& ù
ê úê&ú ¢
ê cos ysinq -siny 0 ú ê q ú = ω (2.A.14)
êë cosq 0 1 úû êëy& úû
To determine whether a solution exists, the determinant of the coefficient matrix on the left may
be evaluated and found to be - sin q . Therefore, there exists a unique solution for Euler angle
time derivatives for any angular velocity ω¢ , except when q is equal to 0 or an integral multiple
of p . In these singular cases, not every angular velocity can be achieved. For those that can be
represented, an infinite family of Euler angle time derivatives results. For analytical purposes,
Eq. (2.A.14) may be solved to obtain
wy¢cos y +wx ¢siny
f& =
sinq
&q = w cos y - w siny (2.A.15)
x¢ y¢
& = w¢z -
y
(w y¢ cos y +wx ¢siny ) cosq
sinq
which shows that a singular case occurs when q is an integral multiple of p , including 0.
2.A.3 Complexity of Computation with Euler Angles
Let a¢ be a vector fixed in a rigid body from the origin of the x ¢-y¢-z¢ frame to point P.
The global vector representation of a¢ is
a = A ( α ) a¢ (2.A.16)
105
where α = [ f q y ] is the vector of Euler angles that define orientation of the body and A ( α )
T
is the orientation transformation matrix given by Eq. (2.A.6). For purposes of illustration, the x
component of a is expanded to obtain
a x = ( cos ycosf - cosqsinfsiny ) a ¢x - ( sinycosf + cosqsinfcos y ) a ¢y + sinqsinfa ¢z (2.A.17)
Differentiating both sides of Eq. (2.A.17) with respect to time yields the x component of velocity
of point P as
a& x = éë( -cos ysinf - cosqcosfsiny ) a ¢x - ( -sinysinf + cosqcosfcos y ) a ¢y + sinqcosfa ¢z ùû f&
+ ëésinqsinfsiny a ¢x + sinqsinfcos y a ¢y + cosqsinfa ¢z ûù q& (2.A.18)
+ ëé( -siny cosf - cosqsinfcos y ) a ¢x - ( cos ycosf - cosqsinfsiny ) a ¢y ùû y
&
Taking the time derivative of both sides of Eq. (2.A.18) yields the x component of
acceleration of point P, including terms that are linear in second time derivatives of Euler angles
and a quadratic form in their first derivatives,
a x = éë( -cos ysinf - cosqcosfsiny ) a ¢x - ( -sinysinf + cosqcosfcos y ) a ¢y + sinqcosfa ¢z ùû &&
&& f
+ ëésinqsinfsiny a ¢x + sinqsinfcos y a ¢y + cosqsinfa ¢z ùû &&
q
+ ëé( -siny cosf - cosqsinfcos y ) a ¢x - ( cos ycosf - cosqsinfsiny ) a ¢y ùû y
&&
(2.A.19)
é f& ù
ê ú
+ ëé f& q& y& ûù Q ( f, q, y, a¢ ) ê q& ú
êy& ú
ë û
where Q ( f, q, y, a¢ ) is the symmetric matrix
é Q11 Q12 Q13 ù

Q ( f, q, y, a¢ ) = ê Q 22 Q 23 ú (2.A.20)
ê ú
êëSym Q33 úû
with elements as follows:

Q11 = ( -cos ycosf + cosqsinfsiny ) a ¢x + ( sinycosf + cosqsinfcos y ) a ¢y - sinqsinfa ¢z
Q12 = sinqsinfsinya ¢x + sinqcosfcos ya ¢y + cosqcosfa ¢z
Q13 = ( sinysinf - cosqcosfcos y ) a ¢x + ( cos ysinf + cosqcosfsiny ) a ¢y
(2.A.21)
Q 22 = cosqsinfsinya ¢x + cosqsinfcos y a ¢y - sinqsinfa ¢z
Q 23 = sinqsinfcos y a ¢x - sinqsinfsiny a ¢y
Q33 = ( -siny cosf + sinqsinfcos y ) a ¢x + ( sinycosf - cosqsinfcos y ) a ¢y
106
It is instructive to compare the expressions for &a&x in Eqs. (2.A.19) through (2.A.21) for
Euler angles with corresponding expressions for Euler parameters. For a = A ( p ) a¢ , using Euler
parameter relations of Section 2.6,
a = B ( p,a¢ ) &&
&& p + B ( p,a
& ¢ ) p& (2.A.22)
Expanding the first component of both sides of Eq. (2.A.22), using p = [ e0 e3 ] and
T
e1 e 2
definitions of Section 2.6, yields
é( e 0 a ¢x - e 2 a ¢y + e3a ¢x ) &&e0 + ( e1a ¢x + e 2a ¢z + e3a ¢y ) &&e1 ù
ax = 2 ê
&& ú
ê + ( e1a ¢y + e0 a ¢x - e3a ¢x ) &&e 2 + ( e1a ¢z - e 0a ¢y - e 2a ¢x ) &&e3 ú
ë û
é 2a ¢x 0 ( a ¢z - a ¢y ) ( a ¢z - a ¢y ) ù (2.A.23)
ê ú
ê 2a x ( a z + a y ) ( a z + a y ) ú &
¢ ¢ ¢ ¢ ¢
+ p& T ê úp
ê 0 -2a ¢x ú
êSym ú
ë 0 û
As expected, acceleration is a linear function of Euler parameter second derivatives, with
coefficients in Eq. (2.A.23) that are linear in Euler parameters. In contrast, the coefficients of
Euler angle second derivatives in Eq. (2.A.19) are products of numerous transcendental functions
(sin and cos) of Euler angles. The contrast between coefficients of the velocity quadratic forms
in Eqs. (2.A.20) and (2.A.21) for Euler angles and in Eq. (2.A.23) for Euler parameters is even
more stark. The Euler parameter coefficients in Eq. (2.A.23) are constants, whereas the
coefficients in Eq. (2.A.21) involve many products of two or three transcendental functions of
Euler angles.
The contrast in derivative complexity between Euler angles and Euler parameters is even
more extreme in equations of kinematic constraint and numerical integration in dynamics.
Kinematic constraint expressions are generally far more complicated than the orientation
transformation used in the above analysis, making calculation of the two derivatives required to
obtain acceleration equations much more intricate. Furthermore, in implementation of implicit
numerical integration methods that are required for stiff mechanical systems in dynamic
simulation, a third derivative of constraint expressions is required. As shown in Sections 2.5 and
2.6, and amplified in Chapter 3, Euler parameter identities based on the quadratic dependence of
orientation transformations make calculation of the numerous required derivatives efficient and
practical. In contrast, calculation of the required derivatives using Euler angles is a nightmare.
107
Appendix 2.B Position and Orientation Code
Brief instructions for use of Codes 2.2, 2.4. 2.5, and 2.6 that implement methods presented
in corresponding sections of Chapter 2 are provided. MATLAB versions of the codes are provided
and made available for use in implementations throughout the text.
2.B.1 Code 2.2 Newton Raphson Solution of k Equations F(x) = 0 for k Variables x
Code 2.2 for solution of k equations F(x) = 0 for k variables x using the Newton-Raphson
method of Section 2.2.7 consists of the main code, lines 1 through 24, and two functions for
definition of F(x) and Fx (x ) shown in Fig. 2.B.1.1. While the code can be used stand alone, it is
intended for incorporation in codes focused on kinematics and dynamics applications, in which
the functions for function evaluation and their Jacobian are generally coded. In application of the
code, the user must select the error tolerance xtol in line 5, the maximum number of iterations to
be allowed itermax in line 6, the function F(x ) = [ F1 (x ) L Fk (x )] in function FEval, and the
T
Jacobian Fx (x) = éë ¶Fi (x) / ¶x j ùû in function FxEval.
1 % Newton Raphson Solution of k Equaitons F(x)=0 for k Variables x

2 %Section 2.2.7
3 %Requires Functions FEval to evaluate F(x) and FxEval to evaluate
4 % Jacobian dF/dx(x)
5 xtol=0.001; % Error tolerance in satisfying F(x)=0
6 itermax=10; %Maximum number of iterations allowed
7 x0=[]; % Initial solution estimate, a k vector
8 x=x0;
9 i=1; %Iteration counter
10 err=xtol+1; %Set initial error tolerance large
11 while err >xtol %Iteration for x, through line 15
12 F=FEval(x); %Evaluate F(x)
13 Fx=FxEval(x); %Evaluate Jacobian dF/dx(x)
14 delx=-Fx\F; %Newton-Raphson iteration
15 x=x+delx;
16 err=norm(F); %Error evaluation
17 if i > itermax %Terminate if iterations exceed maximum allowed
18 warning( 'iteration limit exceeded' );
19 str = string( "iteration limit exceeded ");
20 return
21 end
22 i=i+1;
23 end
24 iter=i-1; %Report number of iteations
1 function F=FEval(x)
2 % Evalyation of F(x)
3 F=[]; %Enter k by 1 matrix [Fi(x)]
4 end
108
1 function Fx=FxEval(x)
2 % Evalyation of Fx(x)=partial derevative matrix of F with respect to x
3 Fx=[]; %Enter k by k matrix [dFi/dxj(x)]
4 end
Figure 2.B.1.1 Newton Raphson Solution of k Equations F(x) = 0 for k Variables x
2.B.2 Code 2.4 Computation of Relative Rotation About Common z¢ -Axes
Code 2.4 for computation of the relative angle of rotation θ i j between x¢i and x ¢j axes for
reference frames with common z¢ axes in Fig. 2.4.8 in Section 2.4.4 is shown in lines 1 through
31 in Fig. 2.B.2.1, using functions for evaluation of vector and matrix operations that are
contained in a library provided in Code 2.6 of Section 2.B.4. The user must provide the body
fixed vectors f i¢, g¢i , and f j¢ , denoted f1pr, g1pr, and f2pr in the code and Euler parameters
p i and p j for the bodies, denoted p1 and p2 in the code.
1 %Given unit vectors f1pr, g1pr and f2pr and Euler parameters p1 and p2
2 %for bodies 1 and 2 with common z1 and z2 axes, evaluate relative rotation
3 %Section 2.4.4
4 %Data are for Numerical Example
5 f1pr=[1;0;0;];
6 g1pr=[0;1;0];
7 f2pr=[1;0;0];
8 p1=[1;0;0;0];
9 e02=cos(pi/4);
10 e2=sin(pi/4)*[0;0;1];
11 p2=[e02;e2];
12 p1d=-0.5*GT(p1)'*[0;0;1];
13 p2d=0.5*GT(p2)'*[0;0;1];
14 %Compute theta12 in Eq. (2.4.37)
15 A1=AT(p1); %AT(p) is the orientation transformation matrix as function of p
16 A2=AT(p2);
17 f1=A1*f1pr;
18 g1=A1*g1pr;
19 f2=A2*f2pr;
20 s=g1'*f2; %definition of sin and cos
21 c=f1'*f2;
22 Arcs=asin(s); %asin(s) is Arcsin(s)
23 if s<=0&&c<0
24 theta12=-pi-Arcs;
25 end
26 if c>=0
27 theta12=Arcs;
28 end
29 if s>=0&&c<0
30 theta12=pi-Arcs;
31 end
Figure 2.B.2.1 Computation of Relative Rotation About Common z¢ -Axes
109
2.B.3 Code 2.5 Definition of A and Solution for p with Given A
Code 2.5 for definition of orientation transformation matrix A and determining p for a
given A using theory presented in Section 2.5 is presented in lines 1 through 50 of Fig. 2.B.3.1,
using functions for evaluation of vector and matrix operations that are contained in a library
provided in Code 2.6 of Section 2.B.4. Part 1 of the code defines the orthogonal orientation
transformation matrix by either direct entry, using the point definition approach presented in
Section 2.5.5, or using data from Euler’s theorem of Section 2.5.3. Part 2 of the code computes a
vector p of Euler parameters from a given orthogonal orientation transformation matrix A, such
that A = A(p) , using results of Section 2.5.4. The code can be used in one call to compute A
using either the point definition approach or Euler’s theorem and in a second call to evaluate p.
1 %AA_Evaluate_Spatial_Orientation_Transformation_Matrix_and_Inverse_for_p
2 %Section 2.5
3 Part=[]; %Part=1, Define orthogonal tramsformation matrix
4 %Part=2, Compute p for given orthogonal tramsformation matrix A
5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%
6 if Part==1 %Define orthogonal tramsformation matrix
7 mode=[]; %mode=1, enter A; mode=2, point definition of A, Section 2.5.5;
8 %mode=3, Euler'S Theorem, Section 2.5.3
9 if mode==1
10 A=[,,;,,;,,]; %Enter the 3x3 orthogonal matrix A
11 end
12 if mode==2 %Define orthogonal transformation matrix A using point
13 %definition of Section 2.5.5.
14 rO=[;;]; %Enter vector rO to origin
15 rP=[;;]; %Enter vector rP to point P
16 rQ=[;;]; %Enter vector rQ to point Q
17 f=(1/norm(rP-rO))*(rP-rO);
18 h=(1/norm(atil(f)*(rQ-rO)))*atil(f)*(rQ-rO);
19 g=-atil(f)*h;
20 A=[f,g,h];
21 end
22 if mode==3 %Define orthogonal transformation matrix A using Euler's
23 %Theorem of Section 2.5.3.
24 %Unit vector u about which rotation angle chi brings x-y-z frame into
25 %coincidence with x'-y'-z' frame
26 ubar=[;;]; %Enter vector ubar about which rotation occurs,
27 %not necessarily normalized
28 chi=[]; %Enter angle chi of rotation
29 u=(1/norm(ubar))*ubar; %Normalization of ubar
30 e0=cos(chi/2);
31 e=sin(chi/2)*u;
32 A=(e0^2-e'*e)*eye(3)+2*e*e'+2*e0*atil(e);
33 end
34 end
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 if Part==2 %Given orthogonal matrix A, compute Euler parameter vector p in
37 %Section 2.5.4 so A=A(p)
38 A=[,,;,,;,,]; %Enter the 3x3 orthogonal matrix A
110
39 [a11,a12,a13,a21,a22,a23,a31,a32,a33] = partA(A);
40 if norm(A'-A)>0
41 trA=a11+a22+a33;
42 e0=0.5*sqrt(trA+1);
43 e=(1/(2*sqrt(trA+1)))*[a32-a23;a13-a31;a21-a12];
44 end
45 if norm(A'-A)==0
46 e0=1;
47 e=zeros(3,1);
48 end
49 p=[e0;e];
50 end
Figure 2.B.3.1 Definition of A and Solution for p with Given A
2.B.4 Code 2.6 Euler Parameter Identities and Derivative Operators
Euler parameter identities and derivative operators of Sections 2.5 and 2.6 are
implemented in the form of MATLAB functions in Fig. 2.B.4.1 and in Code 2.5 for
incorporation into applications throughout kinematics and dynamics of spatial systems.
1 %MATLAB Code Implementing Euler Parameter Derivative Operators of

2 %Sections 2.5 and 2.6 is Contained in Functions that can be Copied
3 %into Application Code
4
5 %Example: f(p)=B'(p,a)*A(p)*b; fsp=K(a,(A(p)*b)+B'(p,a)*B(p,b)
6 a=[1;1;0];
7 b=[0;1;1];
8 p=[1;0;0;0];
9 f=BT(p,a)'*AT(p)*b;
10 fsp=KT(a,AT(p)*b)+BT(p,a)'*BT(p,b);
1 function A=AT(p)
2 %Evaluate A(p) of Eq. (2.5.24), given p
3 e0=p(1);
4 e=[p(2);p(3);p(4)];
5 I3=eye(3);
6 etil=atil(e);
7 A=(e0^2-e'*e)*I3+2*e*e'+2*e0*etil;
8 end
1 function atil=atil(a)
2 %Evaluate til(a) of Eq. (2.1.21), given a
3 atil=[0,-a(3),a(2);a(3),0,-a(1);-a(2),a(1),0];
4 end
1 function BT=BT(p,apr)
2 %Evaluate B(p,apr) of Eq.(2.6.25), given p and apr
3 e0=p(1);
4 e=[p(2);p(3);p(4)];
5 I3=eye(3);
6 etil=atil(e);
7 BT=2*[(e0*I3+etil)*apr,e*apr'-(e0*I3+etil)*atil(apr)];
8 end
111
1 function CT=CT(p,a)
2 %Evaluate C(p,apr) of Eq.(2.6.31), given p and apr
3 e0=p(1);
4 e=[p(2);p(3);p(4)];
5 I3=eye(3);
6 CT=2*[(e0*I3-atil(e))*a,e*a'+(e0*I3-atil(e))*atil(a)];
7 end
1 function E=ET(p)
2 %Evaluate E(p) of Eq.(2.6.1), given p
3 e0=p(1);
4 e=[p(2);p(3);p(4)];
5 E=[-e,atil(e)+e0*eye(3)];
6 end
1 function G=GT(p)
2 %Evaluate G(p) of Eq.(2.6.2), given p
3 e0=p(1);
4 e=[p(2);p(3);p(4)];
5 G=[-e,-atil(e)+e0*eye(3)];
6 end
1 function K = KT(apr,b)
2 %Evaluate K(apr,b) of Eq.(2.6.37), given apr and b
3 K=2*[apr'*b,apr'*atil(b);atil(apr)*b,apr*b'+b*apr'-apr'*b*eye(3)];
4 end
1 function L = LT(a,b)
2 %Evaluate L(a,b) of Eq.(2.6.41), given a and b
3 K=2*[a'*b,-a'*atil(b);-atil(a)*b,a*b'+b*a'-a'*b*eye(3)];
4 end
1 function M=MT(p1,p2)
2 %Evaluate M(p1,p2) of Eq.(2.6.81), given p1 and p2
3 M=2*ET(p1)*GT(p2)';
4 end
1 function N=NT(p1,p2)
2 %Evaluate N(p1,p2) of Eq.(2.6.82), given p1 and p2
3 N=2*GT(p1)*ET(p2)';
4 end
1 function R=RT(a)
2 %Evaluate R(a) of Eq.(2.6.19), given a
3 R=[0,-a';a,atil(a)];
4 end
1 function T=TT(a)
2 %Evaluate T(a) of Eq.(2.6.20), given a
3 T=[0,-a';a,-atil(a)];
4 end
112
1 function Y=YT(a,b)
2 %Evaluate Y(a,b) of Eq.(2.6.83), given a and b
3 Y=-2*GT(ET(a)'*b);
4 end
1 function Z=ZT(a,b)
2 %Evaluate Z(a,b) of Eq.(2.6.83), given a and b
3 Z=-2*ET(GT(a)'*b);
4 end
Figure 2.B.4.1 Euler Parameter Identities and Derivative Operators
113
Appendix 2.C Key Formulas, Chapter 2
114
(Only formulas below with bold italics require p Tp = 1 )
115
116
CHAPTER 3
Kinematics of Multibody Systems
3.0 Introduction
As outlined in Chapter 1, mechanical systems are modeled as collections of rigid bodies
connected by joints between pairs of bodies, to form multibody systems. A broadly applicable
kinematic constraint formulation is presented in Section 3.1, using particles as bodies to illustrate
concepts that form the foundation for progressively more complex planar and spatial multibody
system kinematics in later sections.
Joints between pairs of bodies are characterized by algebraic constraint equations, called
holonomic constraints, comprising a library that can be used to model a spectrum of mechanical
systems. Libraries of the most common planar and spatial joints and associated holonomic
constraints are presented in Sections 3.2 and 3.3. Care is taken to show that constraint equations
derived for each joint imply the geometry of the joint; i.e., to establish the equivalence of
equations used in computation and kinematics of the physical system. Examples of planar and
spatial systems are presented, for subsequent use in computation.
Less common constraints that are stated in terms of velocity were introduced in Section
2.2.6, defined by differential or velocity conditions, called nonholonomic constraints. Examples
of such constraints in more realistic engineering applications are presented in Section 3.4,
including wheels of vehicles that are modeled as rolling on a surface without slipping relative to
the surface; i.e., the relative velocity of points in contact is zero.
Tangent space parameterization of kinematic configuration space is presented in Section
3.5, yielding singularity free constraint manifolds that provide a rigorous differential geometric
foundation for kinematic and dynamic simulation methods presented in this and subsequent
chapters. Basic concepts of differential geometry in a Euclidean space setting that is adequate for
kinematics and dynamics of mechanical systems are presented in Section 3.6, extending local
properties of manifolds defined in Section 3.5 to global singularity free domains of system
kinematic functionality. Useful theoretical results are summarized from a mathematical literature
that is increasingly distant from the realities of kinematics and dynamics of mechanical systems.
Singular configurations of poorly designed or poorly modeled mechanisms are analyzed
in Section 3.7 and the influence of singularities on system kinematics is analyzed. Kinematic
analysis of position, velocity, and acceleration response to time dependent drivers that are
imposed on a system is presented in Section 3.8.
Rudimentary general purpose MATLAB computer codes for kinematic analysis of planar
and spatial systems are outlined in Sections 3.9 and 3.11, respectively, and included in Appendix
3.A. Examples of system kinematic analysis using these computer codes are presented in
Sections 3.10 and 3.12. Readers are encouraged to use these programs to carry out numerical
experiments with models presented and to formulate and analyze additional examples. Finally, a
summary of key formulas used in the chapter is presented in Appendix 3.B.
117
3.1 Holonomic Constraints
The position and orientation of bodies in mechanical systems are represented by
generalized coordinates defined in Sections 2.3 and 2.4 for planar and spatial bodies,
respectively. In preparation for dealing with the intricacies of kinematic constraints that are
formulated in terms of system generalized coordinates for rigid bodies in space, the basic form of
constraints between bodies and their organization as system kinematic constraint equations are
introduced in this section. Concepts are illustrated with the relatively simple form of constraints
between particles, as models of rigid bodies.
3.1.1 Constraints Between Pairs of Bodies
Joints in mechanical systems are physical constructions that limit relative motion of pairs
of bodies identified by indices i and j, with a total number nb of bodies in the system. For
bookkeeping purposes, body j = 0 is designated as the ground body that is fixed in a global
reference frame; i.e., it has constant generalized coordinates relative to that frame. This
numbering convention, with ground as a body, is important in accounting for geometric
quantities that are related to ground attachments in constraint equations and their derivatives.
Constraint equations that characterize joint k = k(i k ,jk ) between bodies i k and jk , k = 1, ..., nb,
involve only generalized coordinates qi Î Rni and q j Î R j that define the position and
n
orientation of the bodies that are connected. In particular, if j = 0, body i is connected to ground
and the constraint equations involve only generalized coordinate q i .
To illustrate constraint formulation with particles, the generalized coordinates of particle i
in the plane are q i = [ x i , yi ] and in space, they are q i = [ x i , y i ,z i ] . The only constraint that acts
T T
between pairs of particles is the scalar distance constraint that specifies the distance d k between
them; i.e.,
Φ k (q i , q j ) = ( (q j - qi ) T (q j - qi ) - d 2k ) / 2 = 0 (3.1.1)
This algebraic constraint on generalized coordinates is called a holonomic constraint (Pars,

T
1965). Defining q ij º éëq iT q Tj ùû , the constraint Jacobian of Eq. (3.1.1) is
Φ kqij (q i , q j ) º éëΦ qk i Φ qk j ùû = éë -( q j - q i )T (q j - q i ) T ùû (3.1.2)
Generalized coordinates are a function of time, t, for a system in motion. Using the chain
rule of differentiation, the time derivative of Eq. (3.1.1) yields the kinematic velocity equation,
T
Φkqij (q i , q j )q& ij = éë -(q j - q i )T (q j - q i )T ùû éëq& iT q& Tj ùû = (q j - q i )T (q& j - q& i ) = 0 (3.1.3)
where over hat ĝ denotes a term that is held constant for the indicated differentiation. Note that
the velocity constraint is linear in velocity, with a coefficient matrix that depends on
configuration. Differentiating Eq. (3.1.3) with respect to time yields the kinematic acceleration
equation,
Φkq (q i , q j )q
ij
(
&&ij + Φqij (q i , q j )qˆ& ij ) qij
q& ij = (q j - qi )T (q
&& j - q
&&i ) + (q& j - q& i )T (q& j - q& i ) = 0
118
or,
(q j - q i )T (&&
q j - &&
qi ) = -(q& j - q& i )T (q& j - q& i ) º -g(q& ij ) (3.1.4)
It is important to note that the acceleration constraint is linear in acceleration, with a coefficient
matrix that depends on configuration, but it also contains a term that is quadratic in velocities.
More generally, a holonomic constraint is defined between planar and spatial rigid bodies
i and j by nh k algebraic equations of the form
F k (q i , q j ) = 0 (3.1.5)
i.e., F k (q i , q j ) Î R nh k . Families of holonomic constraints between planar and spatial bodies are
presented in Sections 3.2 and 3.3, represented by equations of the form of Eq. (3.1.5), with
q i Î R 3 for planar systems and q i Î R 7 for spatial systems. Fora spatial body whose orientation
is defined using Euler parameters, the Euler parameter normalization condition
Φpi (qi ) = ( p Ti p i - 1) / 2 = 0 (3.1.6)
is included in Eq. (3.1.5). In all cases, as in Eq. (3.1.1) for particles, the functions F k (q i , q j )
have as many continuous derivatives as are needed for kinematic anlysis.
The constraint Jacobian of Eq. (3.1.5) is composed of subJacobians with respect to
qi and q j ,
F qki j (q i , q j ) º éF qk i (q i , q j ) F qk j (q i , q j ) ù (3.1.7)
ë û
where Φ qk i (qi , q j ) = Φ k (qi , qˆ j )qi and Φ qk j (qi , q j ) = Φ k (qˆ i , q j ) q j . Note that if body j is ground, j =
0, q j is constant, and Fq j (qi , q j ) = 0 . However, it is important to retain geometric parameters
k
that define the connection of body i with ground in the constraint equation and its derivatives.
Taking the time derivative of Eq. (3.1.5), using the chain rule of differentiation, yields the
kinematic velocity equation, or velocity constraints,
Fqki (qi , q j )q& i + Fqk j (qi , q j )q& j = 0 (3.1.8)
Taking the time derivative of Eq. (3.1.8) yields the kinematic acceleration equation
Fqki (qi , q j )q
&&i + Fqk j (qi , q j )q
&& j = -g k (qi , q j , q& i , q& j ) = -g k (qi j , q& ij ) (3.1.9)
where
(
g k (qi , q j , q& i , q& j ) º F qki (qi , qˆ j )q&ˆ i ) qi
(
q& i + F kq j (q i , qˆ j )q&ˆ j) q& qi
i
(3.1.10)
+ (F k
q& i (qˆ , q )q&ˆ )
i j i q& + ( F
j
k
qj (qˆ , q )q&ˆ ) q&
i j j j
qj qj
119
______________________________________________________________________
Example 3.1.1: Particle p1 is constrained to move on a unit circle with center at point p0 that is
fixed in the x-y plane, as shown in Fig. 3.1.1. Generalized coordinates for the particles are the
constant vector q 0 = [ x 0 y 0 ] º [q 01 q 02 ] and the variable vector q1 = [ x1 y1 ] º éëq11 q12 ùû ,
T T T T
subject to the holonomic constraint

Φ1 (q0 , q1 ) = ( (q1 - q 0 ) T (q1 - q0 ) - 1) / 2 = 0 (3.1.11)
Figure 3.1.1 Particle Constrained to Move on Unit Circle with Center at p 0

The constraint Jacobian is
F1q1 (q1 , q0 ) º éë(q1 - q0 )T 01´2 ùû (3.1.12)
and the velocity equation of Eq. (3.1.8) is

Φ1q1 (q1 , q0 )q& 1 = ( q1 - q0 ) q& 1 = 0
T
(3.1.13)
The term g of Eq. (3.1.10) is thus
(
g1 (q1 , q0 , q& 1 , 0) = F qk1 (q1 , qˆ 0 )q&ˆ 1 ) q1
(
q& 1 = q&ˆ 1T (q1 - q 0 ) ) q1
= q& 1Tq& 1 (3.1.14)
and the acceleration constraint of Eq. (3.1.9) is

&&1 = ( q1 - q 0 ) &&
q1 = -q& 1Tq& 1
T
Φ11 (q1 , q0 )q (3.1.15)
________________________________________________________________________
Suppressing arguments of functions and superscript k for notational simplicity,
expanding matrix differentiation to component form, and changing the order of summation,
multiplication of scalars, and differentiation with respect to scalar variables, yields the following
identity that holds for any vector function a(qi , q j ) :
æ ö
(a qˆ& )
qi i
qj
q& j = å ç å a qa q̂& ia ÷ q& bj = åå aq a qb q& ia q& bj = åå a qb qa q& bj q& ai
b è a
i
øqbj b a
i j
a b
j i
(3.1.16)
æ ö
a è b
j
øq ai
(
= å ç å a qb q&ˆ bj ÷ q& ai = aq j q&ˆ j ) qi
q& i
120
T
where q i = éëq1i L q in i ùû and q j = éëq1j L q j j ùû . Thus, Eq. (3.1.10) reduces to
T
n
(
g k (qi , q j , q& i , q& j ) = F qk i (q i , qˆ j )q&ˆ i ) qi
(
q& i + 2 F qk i (qˆ i , q j )q&ˆ i )
qj
(
q& j + F qk j (qˆ i , q j )q&ˆ j ) qj
q& j (3.1.17)
To obtain an expression for g k , in a form that supports a broad spectrum of kinematic

and dynamic analyses, define
T
χ ij = éë χ Ti χ Tj ùû (3.1.18)
where χ i and χ j have the same dimensions as qi and q j . For joint k, define the operator
(
P2k (qi j , χ i j ) º F qk i j (qi j )χˆ ij )
qi j
é
ë
(
= ê F qk i (q i , qˆ j )χˆ i ) + (F
qi
k
qj (qi , qˆ j )χˆ j ) (F
qi
k
qi (qˆ i , q j )χˆ i ) + (F
qj
k
qj (qˆ i , q j )χˆ j )
ù
qj ú
û
(3.1.19)
A property of the operator P 2(q, χ ) that will be used in many applications is linearity in its
second argument; i.e.,
P2(q, a1χ1 + a 2χ 2 ) = a1P 2(q, χ1 ) + a 2P 2(q, χ 2 ) (3.1.20)
for arbitrary real numbers a1 and a 2 and arbitrary vectors χ1 and χ 2 . This property is a direct
result of linearity of the derivative operator.
Evaluating P 2k (q i j , χ i j )q& ij with ci j = q& ij ,
(
P2 k (q i j , q& i j )q& i j = Fqki (q i , qˆ j )q&ˆ i ) qi
(
q& i + F qk j (q i , qˆ j )q&ˆ j ) qi
q& i
(
+ F qki (qˆ i , q j )q&ˆ i ) qj
(
q& j + Fqk j (qˆ i , q j )q&ˆ j ) qj
q& j (3.1.21)
(
= Fqki (q i , qˆ j )qˆ& i ) qi
(
q& i + 2 F qki (qˆ i , q j )q&ˆ i ) qj
(
q& j + F qk j (qˆ i , q j )q&ˆ j ) qj
q& j
where Eq. (3.1.17) is used. Comparison of Eqs. (3.1.10) and (3.1.21) verifies that
g k (q i j , q& ij ) = P 2 k (q i j , q& ij )q& ij (3.1.22)
3.1.2 Constraint, Velocity, and Acceleration Equations

For a system that is comprised of nb bodies, define system generalized coordinates as
T
q = éëq1T q 2T L q nb
T
ùû Î R ngc (3.1.23)
where qi Î R ni and ngc = å i =1 n i is the total number of generalized coordinates. Numbering

nb
joints with index k = 1, ..., nh, with joint k connecting bodies i k and jk , the constraint equations
of Eq. (3.1.5) may be ordered as
121
é F1 (q i1 , q j1 ) ù
ê ú
F (q ) º ê M ú = 0ÎR
nhc
(3.1.24)
êF nh (qi , q j )ú
ë nh nh û
nh
where nhc = å nh k is the total number of holonomic constraint equations. With this ordering,
k =1
the system constraint Jacobian is
é F1q (q ) ù
ê ú
F q (q ) º ê M ú (3.1.25)
êëF qnh (q ) úû
which contains nonzero entries only in columns i k and jk that correspond to constraint k,
k = 1, ..., nh . The system constraint Jacobian is thus mostly zeros; i.e., it is a sparse matrix.
The system kinematic velocity equations, made up of contributions from Eq. (3.1.8), are
F q (q )q& = 0 (3.1.26)
The derivative operator of Eq. (3.1.21) for the system is made up of contributions from
Eq. (3.1.19),
é ( F1q cˆ ) ù
ê q
ú
P2(q, c ) º ( F q (q )cˆ ) = ê ú (3.1.27)
q
ê nh ú
êë( F q cˆ )q úû
which has the same row and column structure as the Jacobian of Eq. (3.1.25), so it is also sparse.
Finally, system kinematic acceleration equations are comprised of contributions from Eqs.
(3.1.21), (3.1.22), (3.1.27), and (3.1.9),
F q (q )q
&& = -P2(q, q& )q& = -γ(q, q)
& (3.1.28)
While the bookkeeping convention outlined above may seem to be a bit pedantic, it is an
important component of “computational kinematics and dynamics”. If full use is to be made of
the digital computer in forming and solving the equations of motion, the underlying
computations must be systematically organized.
________________________________________________________________________
Example 3.1.2: As an example of a multibody system, consider the three-particle model of a
planar double pendulum shown in Fig. 3.1.2. Generalized coordinates for the particles are
q i = [ x i y i ] º [q i1 q i2 ] , i = 1, 2, and q 0 = 0 for particle 0 that is fixed in ground. Unit
T T
distance constraints act between particle 1 and particle 0 that is fixed to the origin of the x-y
frame and between particles 1 and 2, represented by the holonomic constraints of Eq. (3.1.24),
122
é Φ1 (q0 , q1 ) ù é
ê
( q1Tq1 - 1) / 2 ù
ú
Φ(q1 ,q2 ,q0 ) = ê 2 ú = =0 (3.1.29)
ëΦ (q1 , q2 )û êë( (q2 - q1 ) (q2 - q1 ) - 1) / 2 úû
T
Figure 3.1.2 Three Particle Double Pendulum

T
With q = éëq1T q T2 ùû the constraint Jacobian of Eq. (3.1.25) is
é q1T 01´2 ù
Φ q (q1 ,q 2 ) = ê ú (3.1.30)
ë -( q 2 - q 1 ) (q 2 - q1 )T û
T
and the velocity equation of Eq. (3.1.26) is

é q1T 01´2 ù é q& 1 ù
Φ q (q1 ,q 2 )q& = ê ú = 0 2´1 (3.1.31)
ë -(q 2 - q1 )
T
(q 2 - q1 )T û êëq& 2 úû
The operator of Eq. (3.1.27) is
é( F1q cˆ ) ù é ( q1T cˆ 1 ) ù
ê q
ú ê q
ú é c1T 01´2 ù
P2(q, c ) = = =
ê( F 2 cˆ ) ú ê( -(q - q )Tcˆ + (q - q )Tcˆ ) ú êë c1T - c T2 ú (3.1.32)
-c + c û
T T
ë qû ë qû
q 2 1 1 2 1 2 1 2
and the acceleration constraint of Eq. (3.1.28) is

é q1T 01´2 ù é &&q1 ù é q& 1T q& 1 ù
q=ê
Fq (q )&& Túê ú = - ê ú = - P2( q, q& )q& (3.1.33)
ë -(q2 - q1 )
T
(q2 - q1 ) û ë&&
q2 û ëq& 1 q& 1 - 2q& 1 q& 2 + q& 2 q& 2 û
T T T
________________________________________________________________________
123
Joints between pairs of bodies in a system are represented by algebraic equations that involve
only generalized coordinates of the bodies connected, called holonomic constraints. The
constraint equations, together with Euler parameter normalization conditions for spatial bodies,
comprise the system holonomic kinematic constraints. Generalized coordinate and constraint
numbering conventions that are required for generation and solution of the equations of
kinematics and dynamics on a digital computer are presented and illustrated, using particle
models of mechanical systems. Velocity and acceleration equations are derived, using the
numbering convention presented. In general, kinematic velocity equations are linear in
velocity, with matrix coefficients that depend on configuration. In contrast, acceleration
equations are linear in acceleration, with matrix coefficients that depend on configuration, but
they contain terms that are quadratic in velocities.
Key Formulas
F k (q i , q j ) = 0 F qki j (q i , q j ) º éF qk i (q i , q j ) F qk j (q i , q j ) ù (3.1.5) (3.1.7)
ë û
Fqki (qi , q j )q& i + Fqk j (qi , q j )q& j = 0 (3.1.8)
Fqki (qi , q j )q
&&i + Fqk j (qi , q j )q
&& j = -g k (qi j , q& ij ) (3.1.9)
(
g k (q i j , q& ij ) = F qk i (qi , qˆ j )q&ˆ i ) qi
(
q& i + 2 F qk i (qˆ i , q j )q&ˆ i ) qj
(
q& j + F qk j (qˆ i , q j )q&ˆ j )
qj
q& j (3.1.17)
é
ë
(
P2k (qi j , χ i j ) º ê Fqki χˆ i ) + ( F χˆ ) ( F χˆ ) + ( F χˆ )
qi
k
qj j
qi
k
qi i q
j
k
qj j
qj
ù
úû (3.1.19)
124
3.2 Planar Joints, Constraint Equations, and Drivers
A surprisingly modest library of kinematic joints between pairs of planar bodies, namely
distance, revolute, and translational, enables modeling a wide variety of planar systems. These
and related time dependent driving constraints are presented, in support of planar system
kinematic and dynamic modeling and simulation.
3.2.1 Constraints
For a joint between bodies i and j, constraint equations involve only Cartesian
T T
generalized coordinates q i = éë ri T f i ùû and q j = éë r jT f j ùû . To simplify notation, the
orientation transformation matrix of Eq. (2.3.27) for body l is denoted A º A(f ) . To obtainl l
T
expressions for P2 (qi j , χi j ) of Eq. (3.1.9), define χ i = éëχ χ fi ùû , where χr and χf have the same
k T
ri i i
dimensions as ri and fi , and likewise for body j.

3.2.1.1 Distance Constraint
A planar distance constraint maintains a constant distance d > 0 between points Pi and
Pj on a pair of planar bodies i and j that are located by body fixed vectors s¢i and s¢j . It is a
massless coupler of length d > 0 shown in Fig 3.2.1. Physically, it can be envisioned as a bar of
length d > 0 with ball and socket joints at each end. With the vector from Pi to Pj ,
di j º rj + Ajs¢j - ri - Ais¢i (3.2.1)
the distance constraint equation is the scalar condition

Fdist (qi , q j ) º ( dTij di j - d 2 ) / 2 = 0 (3.2.2)
Figure 3.2.1 Distance Constraint Between Bodies i and j
If body i is held fixed, point Pj can move on a circle of radius d > 0 and center P and i
body j is free to rotate about point Pj ; i.e., body j has two degrees of freedom. Thus, a planar
distance constraint eliminates one degree of freedom from the pair.
125
Using Eq. (2.3.31) subJacobians of the distance constraint are
F qdist
i
= - éë d ijT d ijT PA i s ¢i ùû
(3.2.3)
F qdistj = éë d ijT d ijT PA js ¢j ùû
and terms in the operator of Eq. (3.1.19) are
(F dist
qi χˆ i ) qi
(
= - d ijT χˆ ri + d Tij PA i s¢i χˆ fi ) qi
( ˆ T PTd
= - χˆ rTi d i j + dˆ Tij PA i s¢i χˆ fi + χ fTi s¢i T A i ij ) qi
= a [ I2
T
i PA i s¢i ] + d éë0 χ fi A i s¢i ùû
T
ij
(F )
(3.2.4)
dist
qj χˆ j = -a Tj [ I 2 PA is¢i ]
qi
(F dist
qi χˆ i ) qj
= -a Ti éë I 2 PA js¢j ùû
(F dist
qj χˆ j ) qj
= a Tj ëé I 2 PA js¢j ûù - d ijT ëé0 χ f j A js¢j ûù
where ai = χr + χf PAis¢i and a j = χr + χf PAjs¢j . Thus,

i i j j
P 2dist (qi j ,c i j ) = éë P 2idist P 2dist

j û
ù (3.2.5)
where
P2dist
i = éë(ai - a j )T (ai - a j ) T PAi s¢i + χ fi dTij Ais¢i ùû
P2dist
j = éë(a j - ai )T (a j - ai ) T PA js¢j + χ fj dijT A js¢j ùû
The right side of the acceleration equation is thus

γ dist = P2dist (q i j ,&q ij )&q ij (3.2.6)
3.2.1.2 Revolute Constraint
A revolute joint between planar bodies i and j allows relative rotation about a point P that
is common to bodies i and j, as shown in Fig. 3.2.2. Physically, such a joint is a rotational
bearing between the bodies at point P. The revolute joint constraint is the vector condition
Φrev (qi , q j ) º di j = rj + A js¢j - ri - Ais¢i = 0 (3.2.7)
If one body is held fixed, the other body has a single rotational degree of freedom. Thus, a
revolute joint eliminates two degrees of freedom from the pair.
126
Figure 3.2.2 Revolute Between Bodies i and j
To see that Eq. (3.2.7) implies the geometry of the joint, note that vectors locating point P
on bodies i and j are ri = ri + s¢i = ri + As ¢ ¢
i i and rj = rj + s j = rj + Ajs j . Thus, Eq. (3.2.7) implies
P P
riP = rjP , which is the definition of the revolute joint.
SubJacobians of the revolute constraint are

Φqrevi = - [ I PA is¢i ]
(3.2.8)
Φqrevj = éëI PA js¢j ùû
(Φ rev
qi χˆ i ) qi
= éë0 χ fi A is¢i ùû
( Φ χˆ )
rev
qj i
qi
=0
(3.2.9)
(Φ rev
qi χˆ i ) qj
=0
( Φ χˆ )
rev
qj i
qj
= é0 -χ f j A js¢j ù
ë û
Thus,
P2rev (qi j ,χ i j ) = éëéë0 χ f Ais¢i ùû éë0 -χ f A js¢j ùû ùû

i j
(3.2.10)
and the acceleration right side is

γ rev = P2rev (qi j ,&qij )&qij (3.2.11)
3.2.1.3 Translational Constraint
A translational joint, or prismatic joint, between planar bodies i and j is shown in Fig.
3.2.3. It allows relative translation of the bodies along a common axis that is defined by vectors
vi in body i and v j in body j that emanate from points Pi and Pj , respectively, but no relative
rotation of the bodies. Physically, such a joint may be defined as a straight key on one body that
fits precisely in a straight slot (or keyway) in the second body and allows relative translation
along their common center lines. If one body is held fixed, the other body has only a single
127
translational degree of freedom. Thus, a translational joint eliminates two degrees of freedom
from the pair.
Figure 3.2.3 Translational Joint Between Bodies i and j

For the translational joint shown in Fig. 3.2.3, points P and P are specified on the path of
i j
relative translation between bodies i and j by body fixed vectors s¢i and s¢j . Nonzero body fixed
vectors v¢i and v¢j emanate from points P and P and lie on the axis of relative translation. While
i j
not required, it is recommended that v¢i and v¢j be unit vectors. The vector dij connects points
P and Pj . The geometric definition of the translational joint is that vectors vi
i
and v j must remain
collinear. To have these vectors collinear, it is sufficient that dij and v j be perpendicular to
vi = Pvi , which is perpendicular to vi . Transforming v ¢i to the global frame,
^ ^
vi^ = Ai vi¢^ = Ai Pv¢i (3.2.12)

^
The translational joint conditions that dij and v j are perpendicular to vi are thus
évi^Tdij ù é v¢iT PATi dij ù

Φ (qi , q j ) º ê ^T ú = - ê T T
tran
ú=0 (3.2.13)
ë vi v j û ëv¢i PAi A jv¢j û
As long as dij ¹ 0 , Eq. (3.2.13) implies that the vectors v i , v j , and d ij are parallel and,
since they have points in common, vi and v j are collinear. If dij = 0 , which is possible, vi and v j
emanate from the same point and, by the second of Eqs. (3.2.13), they are parallel. Thus, they are
collinear and Eqs. (3.2.13) are equivalent to the geometry of the translational joint.
SubJacobians of the translational joint constraint are
128
é v ¢i T PA iT - v ¢i T s¢i - v ¢i T A iT d i j ù
Φ qtran = ê ú
i
ëê 0 - v ¢i T A iT A j v ¢j ûú
(3.2.14)
é - v ¢T PA iT v ¢i T A iT A js¢j ù
Φ qtran =ê i ú
j
ëê 0 v ¢i T A iT A j v ¢j ûú

é χ fi v¢i T A Ti v¢i T A Ti χ ri + χ fi v¢i T AiT Pdi j + χ fi v¢i T Ps¢i ù
(Φ tran
qi χˆ i ) =ê
0 χ fi v¢i T AiT PA j v¢j
ú
qi
êë úû
é0 - v¢i T A Ti χ rj - χ f j v¢i T A Ti PA js¢j ù
(Φ tran
qj χˆ j ) qi
=ê
êë0 -χ f j v¢i T AiT PA j v¢j
ú
úû
(3.2.15)
é -χ fi v¢i T ATi -χ fi v¢i T AiT PA js¢j ù
(Φ tran
qi χˆ i ) =ê
0
ú
-χ fi v¢i T A Ti PA j v¢j ûú
ëê
qj
é 0 χ f j v¢i T AiT PA js¢j ù

(Φ tran
qj χˆ j ) qj
=ê
êë 0 χ f j v¢i Ai PA j v¢j úû
T T
ú
Thus,
P 2tran (q i j ,χ i j ) = éë P 2itran P 2tran
j û
ù (3.2.16)
where
P2 tran
éχ f v¢i T AiT
=ê
i
( )
v¢i T AiT χ ri - χ rj + χ fi Pdi j - χ f j PA js¢j + χ fi v¢i T Ps¢i ù
ú
ê
( ) ú
i
êë 0 χ fi - χ f j v¢i Ai PA j v¢j
T T
úû
P2 tran
é-χ f v¢i T AiT
=ê
i
(χ fj )
- χ fi v¢i T AiT PA js¢j ù
ú
ê
(χ ) ¢ú
j
0 ¢
f j - χ fi v i A i PA j v j ú
T T
ëê û
The acceleration right side is thus
γ tran = P 2 tran (q i j ,&q ij )&q ij (3.2.17)
3.2.2 Kinematic Drivers

Kinematic constraints can become kinematic drivers by specifying time dependent terms
that define the desired relative motion. The distance driver is obtained by making d a function of
time, d(t) > 0 , in the distance constraint of Eq. (3.2.2); i.e.,
(
FdistD = dTijdi j - d ( t ) / 2 = 0
2
) (3.2.18)
129
(q i , q j ) , P2 (qi j , χ i j ) , and g (qi j , q& ij ) are the same as in Eqs. (3.2.3), (3.2.6), and
distD distD
While ΦdistD
q ij
(3.2.7), the velocity and acceleration equations of Eqs. (3.1.8) and (3.1.9) must be modified to
account for the time dependent term in Eq. (3.2.18); i.e.,
Φ distD
qi (q i , q j )q& i + Φ distD & j = d ( t ) d& ( t )
q j (q i , q j )q
(3.2.19)
Φ qdistD
i
q i + Φ distD
(q i , q j )&& q j = -g distD (q i j , q& ij ) + d ( t ) &&
q j ( q i , q j ) && d ( t ) + d& ( t ) d& ( t )
In order to specify relative motion in a translational joint, a distance driver may be
imposed between points Pi and Pj of Fig. 3.2.3.
A relative rotation driver associated with a revolute joint can be defined by specifying
the relative rotation b(t) of bodies that are connected by the revolute joint; i.e., the constraint of
Eq. (3.2.7) is augmented with
ΦrotD º fj -fi - b( t ) = 0 (3.2.20)
The velocity and acceleration equations for this driver are simply
f& j - f& i = b& ( t )
(3.2.21)
&&
fj - && b( t )
fi = &&
In general, when driving constraints are of the form
ΦD (q, t) = Φ(q) + f (t) = 0 (3.2.22)
the associated velocity and acceleration constraint equations are
Φ qD ( q, t)q& + Φ Dt ( q, t) = Φq ( q )q& + f& (t) = 0 (3.2.23)
q + γ(q,q)
Φ qD ( q, t)&& & + Φ Dtt ( q, t) = Φq (q )&& & + &&
q + γ(q, q) f (t) = 0 (3.2.24)
Just as subroutines in kinematic analysis computer code are required for evaluation of Φ q (q )
& for all constraints, subroutines are required for evaluation of f (t), f& (t), and &&f (t) for
and γ(q, q)
driving constraints.
3.2.3 Planar System Examples
3.2.3.1 Double Pendulum
The planar double pendulum of Fig. 3.2.4 is made up of two bars, each of length two
units, designated as bodies 1 and 2. Body 1 is connected to ground (body 0) at its left end with a
revolute joint and to body 2 at its right end with a revolute joint. Data for the joints are tabulated
as follows
rev k = 1: i1 = 1, s1¢ = -u¢x ; j1 = 0, A 0 = I , s¢0 = 0
(3.2.25)
rev k = 2 : i 2 = 1, s1¢ = u¢x ; j2 = 2, s¢2 = -u¢x
130
where u¢x is a unit vector along the positive x ¢ axis and k is the constraint index. It is important
to be consistent in use of indices i and j in the joint definitions of Eq. (3.2.25), since they define
variables in the constraint equations and associated subJacobians and P 2(q , χ ) operators.
Figure 3.2.4 Planar Double Pendulum

T
With q = éë r1T f1 r2T f2 ùû Î R 6 , from Eq. (3.2.7), the constraint equations are
é Φ rev1 ( q1 ) ù é -r1 + A1u ¢x ù

Φ ( q ) = ê rev2 ú = êr - A u ¢ - r - A u ¢ ú = 0 (3.2.26)
ë Φ ( q1 , q 2 ) û ë 2 2 x 1 1 xû
This is a system of four nonlinear equations in six generalized coordinates. If the constraint
Jacobian has full rank, the system should have two degrees of freedom. From Eq. (3.2.8), the
constraint Jacobian is
éFqrev1 0 ù é I -PA1u¢x 0 0 ù
Φq (q) = ê rev2
1
ú=ê
I -PA 2u¢x úû
(3.2.27)
¢
êëFq1 Frev2
q2 ú û ë -I -PA1u x
Since the submatrix composed of the first and third submatrix columns ha determinant 1, the
Jacobian has full rank. Finally, from Eq. (3.2.11),
é f& 12 A 1u ¢x ù
g=ê 2 ú (3.2.28)
& &2
ë f1 A 1u ¢x + f 2 A 2 u ¢x û
3.2.3.2 Slider-Crank, 3-Body Model
In the three body model of the planar slider-crank mechanism shown in Fig. 3.2.5, body
1 is the crank of radius R. It is constrained by revolute joint 1 with ground (body 0); body 2 is the
connecting rod of length 2 units, with revolute joints at the ends that are connected to bodies 1
and 3; and body 3 is the slider that translates without rotation along the global x axis in ground.
Data for the joints are tabulated as follows:
rev k = 1: i1 = 1, s1¢ = 0; j1 = 0, A 0 = I , s¢0 = 0
tran k = 2 : i 2 = 3, v¢3 = u¢x , s¢3 = 0; j2 = 0, A 0 = I, v¢0 = u¢x , s¢0 = 0
(3.2.29)
rev k = 3 : i3 = 1, s1¢ = Ru¢x ; j3 = 2, s¢2 = -u¢x
rev k = 4 : i 4 = 2, s¢2 = u¢x ; j4 = 3, s¢3 = 0
131
Figure 3.2.5 Planar Slider-Crank, Three Body Model
T
With q = éë r1T f1 r2T f2 r3T f3 ùû Î R 9 , from Eqs. (3.2.7) and (3.2.13), the constraint
equations are
é r1 ù
é Φ1 (q1 ) ù ê ú
ê ú ê -u¢x Pr3
T
ú
Φ (q 3 ) ú
2
Φ (q ) = ê 3 =ê -u¢x T PA 3u¢x ú=0 (3.2.30)
ê Φ (q1 , q 2 ) ú ê ú
ê 4 ú êr2 - A 2 u¢x - r1 - RA1u¢x ú
êëΦ (q 2 , q3 ) úû ê ú
ë r3 - r2 - A 2 u¢x û
This is a system of eight nonlinear equations in nine generalized coordinates. If the constraint
Jacobian has full rank, the system should have one degree of freedom.
While the Jacobian and matrix P 2( q , χ ) for the planar double pendulum have some zero
entries, sparsity is more significant for the three body slider-crank. Block submatrices in Eq.
(3.2.31) that are left blank are all zeros. From Eqs. (3.2.8) and (3.2.14), the constraint Jacobian is
é Φ1q1 ù é I 0 ù
ê 2 ú ê ú
-u¢x PT
0 ú
ê Φq ú ê
Φq (q) = ê 3 3 ú = ê 0 u¢x T A3u¢x ú (3.2.31)
ê Φq12 ú ê -I -RPA u¢ I -PA 2u¢x
ú
êΦ 4 ú ê 1 x ú
ë q 23 û êë -I -PA 2u¢x I 0 úû
Finally, from Eqs. (3.2.11) and (3.2.17),
é 0 ù
é g 1 (q1 , q& 1 ) ù ê ú
ê ú ê 0 ú
g 2
( q , &
q )
g (q, q& ) = ê 3 3 3 ú = ê f& 2u¢ T PA u¢ ú (3.2.32)
ê g (q1 , q& 1 , q 2 , q& 2 ) ú ê 2 3 x 3 x
ú
ê 4 ú ê1 &
f A u ¢ + &
f ¢
2 A 2u x ú
2
R
êë g (q 2 , q& 2 , q 3 , q& 3 ) úû ê
1 x
ë f&22 A 2u¢x ûú
3.2.3.3 Slider-Crank, 2-Body Model with Rotation Driver
In the two-model of the planar slider-crank mechanism shown in Fig. 3.2.6, body 1 is the
crank of radius R. It is constrained by revolute joint 1 with ground (body 0). Body 2 is the slider
that translates along the global x axis in ground. The connecting rod is modeled as a distance
132
constraint between bodies one and two. A rotation driver that specifies the angle of rotation q(t)
of the crank relative to ground is imposed to specify the motion of the system. Data for the joints
and driver are tabulated as follows:
rev k = 1: i1 = 1, s1¢ = 0; j1 = 0, A0 = I, s¢0 = 0
tran k = 2 : i2 = 2, v¢2 = u¢x , s¢2 = 0; j2 = 0, A0 = I, v¢0 = u¢x , s¢0 = 0
(3.2.33)
dist k = 3: i3 = 1, s1¢ = Ru¢x ; j3 = 2, s¢2 = 0, d = 2
rotD k = 4 : i4 = 1, s1¢ = 0; j4 = 0, A0 = I, s¢0 = 0; f1 - q(t) = 0
Figure 3.2.6 Planar Slider-Crank, Two Body Model

T
With q = éë r1T f1 r2T f2 ùû Î R 6 , from Eqs. (3.2.2), (3.2.7) and (3.2.13), the constraint
equations are
é r1 ù
é Φ1 (q1 ) ù ê -u¢ T Pr ú
ê 2 ú
Φ (q2 ) ú êê -u¢ T PA u¢ úú
x 2
Φ(q,t) = 3 ê = x 2 x =0 (3.2.34)
êΦ (q1 , q2 ) ú ê T ú
ê 4 ú ê( d12d12 - 4 ) / 2 ú
ë Φ (q1 ,t) û ê ú
ë f1 - q(t) û
where d12 = r2 - r1 - RAu ¢
1 x . This is a system of six nonlinear equations in six generalized
coordinates and time. If the constraint Jacobian is nonsingular, it should have a unique solution
for the generalized coordinates as functions of time. From Eqs. (3.2.3), (3.2.8), and (3.2.14), the
constraint Jacobian is
é Φ1q1 ù é I 0 0 0 ù
ê 2 ú ê 0 0 -u¢x T P 0 ú
ê Φq ú ê ú
Φ q (q , t) = ê 3 2 ú = ê 0 0 0 u ¢x T A 2u ¢x ú (3.2.35)
êΦ q12 ú ê -d T - R d12 PA1u¢x
T
d12
T
0
ú
ê Φ 4 ú ê 12 ú
ë q1 û êë 0 1 0 0 úû
Finally, from Eqs. (3.2.6), (3.2.11), and (3.2.17),
133
é 0 ù
é g1 (q1 , q& 1 ) ù ê ú
ê ú ê 0 ú
g 2
(q , &
q )
g (q, q& ) = ê 3 2 2 ú = êf& 2u¢ T PA u¢ ú (3.2.36)
ê g (q1 , q& 1 , q 2 , q& 2 ) ú ê 2 x 2 x
ú
ê ú ê b ú
ëê g (q1 , q& 1 ) ûú ê
4
ë 0 úû
where b = &r1T&r1 + &f12 ( R 2 + Rd12

T
A1u¢x ) + &r2T&r2 - 2R&f1u xT¢ A1T Pr
& 1 + 2R&f1u Tx ¢ A1T Pr
& 2 - 2&r1T&r2 .
3.2.3.4 Windshield Wiper

The windshield wiper mechanism of Fig. 1.2.3 is modeled in the plane, with bodies and
constraints shown in Fig. 3.2.7. Three bodies are pivoted in body 0 (ground), which is the vehicle
chassis, and rotate about revolute joints, as shown. Body 1 is the crank. Bodies two and three are
rocker arms that control rotation of the pair of windshield wipers shown in Fig. 1.2.3.
Connecting rods of lengths 70 cm and 100 cm are modeled as distance constraints between
bodies connected. Data for the joints are tabulated as follows:
rev k = 1: i1 = 1, s1¢ = 0; j1 = 0, A 0 = I, s¢0 = 0
rev k = 2 : i2 = 2, s¢2 = 0 j2 = 0, A 0 = I, s¢0 = -70u x
rev k = 3 : i3 = 3, s¢3 = 0 j3 = 0, A 0 = I, s¢0 = 30u x (3.2.37)
dist k = 4 : i 4 = 1, s1¢ = -5u¢y ; j4 = 2, s¢2 = -8u¢y , d = 70
dist k = 5 : i5 = 2, s¢2 = 5u¢y ; j3 = 3, s¢3 P = 5u¢y , d = 100
Figure 3.2.7 Windshield Wiper Mechanism

T
With q = éë r1T f1 r2T f2 r3T f3 ùû Î R 9 , from Eqs. (3.2.7) and (3.2.2), the constraint
equations are
é Φ1 ( q1 ) ù é r1 ù
ê 2 ú ê r2 + 70u x ú
ê Φ ( q 2 ) ú ê ú
Φ( q ) = ê Φ 3 ( q 3 ) ú = ê r3 - 30u x ú=0 (3.2.38)
ê ú
ê Φ (q1 , q 2 ) ú ê ( 12 12
ê 4 ú d T d - 702 ) / 2 ú
êëΦ5 (q 2 , q 3 )úû ê( d T d - 1002 ) / 2ú
êë 23 23 úû
134
This is a system of eight nonlinear equations in nine generalized coordinates. If the constraint
Jacobian has full rank, the system should have one degree of freedom.
While Eqs. (3.2.3) and (3.2.8) can be used to expand the 8 ´ 9 Jacobian, the explicit form
of the matrix is not needed. The constraint Jacobian is numerically evaluated using computer
routines in MATLAB Code 3.9 of Appendix 3.A. Similarly, the 8-vector γ could be written,
using Eqs. (3.2.6) and (3.2.11), as in the prior examples. It would be rather complex and is not
needed in explicit form, since it can be evaluated numerically using routines in Code 3.9.
Numerical examples of kinematic analysis using these and other models are presented in
Section 3.10, using MATLAB Code 3.9 of Appendix 3.A.
A family of three kinematic constraints and associated time dependent drivers provide the
foundation for modeling and simulation of a broad spectrum of planar multibody systems. The
associated holonomic constraint equations and derivatives derived are implemented in
MATLAB Code 3.9 of Appendix 3.A and used in examples presented in Section 3.10. Models
of four planar systems are presented to illustrate use of the library of constraints and provide
kinematic equations that are used later in the chapter for numerical examples.
Key Formulas
F dist º ( dTij di j - d 2 ) / 2 = 0 di j º rj + Ajs¢j - ri - As ¢
i i (3.2.2) (3.2.1)
é v¢i T PAiTdij ù
Φ º di j = rj + A js¢j - ri - Ais¢i = 0
rev
Φ tran
º -ê T T ú = 0 (3.2.7) (3.2.13)
ëê v¢i PAi A j v¢j ûú
135
3.3 Spatial Joints, Constraint Equations, and Drivers
In contrast with the modest complexity of planar system kinematic modeling, a wide
variety of joint designs are available for application in spatial systems. The library of spatial
kinematic joints is thus large and constraint equations that define their geometry are functions of
seven generalized coordinates per body, in contrast to just three for planar bodies. The algebra
and calculus involved in formulating spatial constraint equations and their derivatives is intricate
and can only be carried out using the identities derived in Section 2.6 and computer code that
implements them in MATLAB Code 2.6 of Appendix 2.C. Fortunately, complexity can be
managed using a library of four building block constraints that support the breadth of spatial
kinematic constraints.
3.3.1 Structure of Constraint Equations
Body fixed x¢-y¢-z¢ and x ¢¢-y¢¢-z ¢¢ reference frames shown in Fig. 3.3.1 are used to locate
points such as Pi and Pj on bodies i and j and vectors si , s j , ai , and aj that are fixed in bodies i
P P
and j and used to formulate kinematic constraint equations. The orientation of the x ¢i -y¢i -z¢i frame
relative to the global x-y-z frame is specified by the Euler parameter vector p i , similarly for
body j. The constant orthogonal transformation matrix Ci transforms a vector a ¢¢i represented in
the x ¢¢i -y ¢¢i -z ¢¢i frame to the x ¢i -y¢i -z¢i frame in body i, and similarly for body j. Generalized
coordinate vectors that define the location and orientation of bodies i and j, relative to the global
T T
x-y-z frame, are Cartesian generalized coordinates q i = éëriT pTi ùû and q j = éërjT p Tj ùû .
Figure 3.3.1 Geometry of Bodies in Space

The organization of spatial constraint equations and their derivatives is as presented in
T
Section 3.1, with the exception that vector χ i is partitioned as χ i = éë χ rTi χ pTi ùû , where the
dimensions of χ ri and χ p i are the same as ri and p i , and likewise for body j. For calculation
T
of derivatives of constraints with respect to generalized coordinates, q i j º éëq i T q jT ùû . To
136
simplify notation, the orientation transformation matrix of Eq. (2.5.22) for body l is denoted
A l º A (p l ) .
3.3.2 Building Block Constraints
Four building block constraints provide the foundation for representing a variety of
spatial joints between pairs of bodies. Based on vectors shown in Fig. 3.3.1, they are as follows:
(1) distance constraint specifies the positive distance between points Pi and Pj
(2) spherical constraint specifies that points Pi and Pj coincide
(3) dot1 constraint specifies that nonzero body fixed vectors a i and a j are orthogonal
(4) dot2 constraint specifies that nonzero body fixed vector a j and vector d i j are orthogonal
3.3.2.1 Distance Constraint

The distance constraint shown in Fig. 3.3.2 consists of a massless coupler between
bodies i and j that contains spherical joints at each end, attached to bodies i and j at points
Pi and Pj . The joint is defined by locating joint definition points Pi and Pj in bodies i and j with
body fixed vectors s¢i and s¢j , and by specifying the distance d > 0 between points Pi and Pj on
the coupler, as shown in Fig. 3.3.2.
Figure 3.3.2 Geometry of the Spatial Distance Constraint

The geometry of the spatial distance constraint is simply that the distance between points
Pi and Pj is equal to d > 0 ; i.e.,
Fdist ( Pi ,Pj ,d ) º ( dijTdij - d 2 ) / 2 = 0 (3.3.1)
where
d ij = rj + A js¢j - ri - A is¢i (3.3.2)
137
Equation (3.3.1) guarantees that the distance between points Pi and Pj is equal to d > 0 , so it
implies the geometry of the joint. The only danger in using this constraint is that it will introduce
serious singularities in computation if the user specifies d = 0 .
Since this joint is characterized by a scalar constraint equation, it should allow five
relative degrees of freedom between the bodies connected. From Fig. 3.3.2, it is clear that if body
j is fixed in space, then point Pi can move freely on a two dimensional spherical surface of
radius d, with center at point P j . In addition, body i has three orientation degrees of freedom, for
a total of five relative degrees of freedom of bodies connected by the joint.
Differentiating Eq. (3.3.1) yields the constraint subJacobians
ˆT ( Tˆ ˆT ˆT )
q i = (d ij d i j )q i + (d ij d ij )q i / 2 = (d ij d i j )q i + (d ij d i j )q i / 2
Φdist ( )
= d Tij (d i j )qi = -d Tij éë I B ( pi ,s¢i ) ùû
( ) ( )
(3.3.3)
ˆT Tˆ ˆT ˆT
q j = (d ij d i j )q j + (d ij d ij )q j / 2 = (d ij d i j )q j + (d ij d i j )q j / 2
Φdist
= d Tij (d i j )q j = d Tij ëé I B p j ,s¢j ûù ( )

where the derivative operator B(p, s¢) is given by Eq. (2.6.25) and terms in Eq. (3.1.19) are
(F dist
qi i χ̂ ) qi
(
= - d Tij χ ri + d Tij B ( pi ,s¢i ) χ pi ) qi
(
= - χ rTi d i j + χ pTi BT ( pˆ i ,s¢i ) d i j + dˆ Tij B χ pi ,s¢i pi ( ) ) qi
= é( χ T
) ( χ + χ B ( p ,s¢ )) B ( p ,s¢ ) - d B ( χ ,s¢ )ùû
+ χ pTi BT ( pi ,s¢i ) T T T T
ë ri ri pi i i i i ij pi i
= éb b B ( p , s¢ ) - d B ( χ , s¢ ) ù
T T T
ë i i û i i ij pi i
(F dist
qi i χ̂ ) = - éë( χ + χ B ( p ,s¢ ) ) ( χ + χ B ( p ,s¢ ) ) B ( p ,s¢ ) ùû
qj
T
ri
T
pi
T
i i
T
ri
T
pi
T
i i j j
= - éëb b B ( p ,s¢ )ûù T

i
T
i j j (3.3.4)
(F dist
qj χ̂ ) = - é( χ + χ B ( p ,s¢ ) ) ( χ + χ B ( p ,s¢ ) ) B ( p ,s¢ )ù
j
ë qi
T
rj
T
pj
T
û
j j
T
rj
T
pj
T
j j i i
= - ëébTj b Tj B ( pi ,s¢i ) ûù
(F χ̂ )
dist
qj j
qj
(
= é χ rTj + χ pTj BT p j ,s¢j
ë ( )) ( χ T
rj ( )) (
+ χ pTj BT p j ,s¢j B p j ,s¢j + d Tij B c p j ,s¢j ù
û ) ( )
= éb Tj
ë ( )
bTj B p j ,s¢j + d ijT B χ p j ,s¢j ù
û ( )
where
b i º χ ri + B(pi , s¢i )χ pi
b j º χ rj + B(p j , s¢j )χ p j
Substituting from Eqs. (3.3.4) into Eq. (3.1.19),
138
P2dist (q i j , c i j ) = éë P2dist
i P2dist
j û
ù (3.3.5)
where
P2dist
i (
= Φdist ˆ
qi χ i ) + (Φ
qi
dist
qj χˆ j ) qi
= éëbiT ( )
biT B ( pi ,s¢i ) - d Tij B χ pi ,s¢i ùû + éëb Tj b Tj B ( p i ,s¢i )ùû
= éë(bTi - bTj ) (b iT - b Tj )B(p i , s¢i ) - d ijT B(χ pi , s¢i )ùû (3.3.6)
P2dist
j = ëé(bTj - b iT ) (b Tj - b iT )B(p j , s¢j ) + dijT B(χ p j , s¢j ) ûù
The right side of the acceleration constraint is thus

γ dist = P2dist (qi j ,q& ij )q& ij (3.3.7)
This is typical of simplifications that are made possible through use of derivative operators of
Section 2.6.
3.3.2.2 Spherical Constraint
The spherical joint (or ball and socket joint) shown in Fig. 3.3.3 is defined by the
geometric condition that the center of the ball at point Pi , located by the body fixed vector s ¢i on
body i, coincides with the center of the socket at P j on body j, located by s ¢j . This condition is
simply the vector equation
Fsph ( Pi ,Pj ) º di j = rj + A js¢j - ri - Ai s¢i = 0 (3.3.8)
Equation (3.3.8), which is equivalent to the spherical joint, can be viewed as three scalar
equations in the three components of the vectors involved. It thus eliminates three relative
degrees of freedom between the bodies connected. This can be seen from Fig. 3.3.3. If body j is
fixed in space, body i can rotate freely about the common point P, hence it has three degrees of
orientation freedom relative to body j.
Figure 3.3.3 Geometry of the Spherical Joint
139
qi = - é
F sph ë I B ( pi ,s¢i ) ùû
(3.3.9)
é B ( p j ,s¢j ) ù
q j = ëI
F sph û
and terms in Eq. (3.1.19) are
(F sph
qi )
cˆ i
qi ë (
= é 0 -B c pi ,s¢i ù
û )
(F sph
qi cˆ )
i
qj
=0
(F )
(3.3.10)
sph
qj cˆ j =0
qi
(F sph
qj cˆ )
j
qj ë (
= é0 B c p j ,s¢j ù
û)
Thus,
P 2 sph (q i j , χ ij ) = é ëé 0 -B (χ pi , s¢i ) ûù é 0 B (χ p , s¢j ) ù ù

ë ûû
(3.3.11)
ë j
And the right side of the acceleration equation is

& ij )p
γsph = P2sph (qi j ,p & ij (3.3.12)
3.3.2.3 Dot 1 Constraint

The dot1 constraint is that nonzero vectors a i and a j fixed in bodies I and j are
orthogonal; i.e.,
Fdot1 (ai , a j ) = a¢iT AiTA ja¢j = 0 (3.3.13)

Fdot1 é ¢T T ¢ ù
qi = ë 0 a j A j B(pi , ai ) û
(3.3.14)
ë0 a¢i Ai B(p j , a¢j ) ùû
qj = é
Fdot1 T T
(F dot1
qi cˆ )
i q
i
= éë0 a¢jT A Tj B(cpi , a¢i ) ùû
(F dot1
qi cˆ )
i q
j
= éë 0 c Tpi BT (p i , a¢i )B(p j , a¢j ) ùû
(F )
(3.3.15)
dot1
qj cˆ j = éë 0 cpTj BT (p j , a¢j )B(pi , a¢i ) ùû
qi
(F dot1
qj cˆ ) j
qj
= éë0 a¢i T A Ti B( cp j , a¢j ) ùû
Thus,
P 2dot1 (q i j , χ ij ) = éëP 2idot1 P 2dot1
j
ùû (3.3.16)
140
where
P2dot1
i = éë 0 a¢jT A Tj B( cpi , a¢i ) + cpT j B T ( p j , a¢j ) B( p i , a¢i ) ùû
P2dot1
j = éë 0 a¢i T A Ti B( cp j , a¢j ) + cpTi B T ( p i , a¢i )B(p j , a¢j ) ùû
and the right side of the acceleration equation is

& ij )p
γdot1 = P2dot1 (qi j ,p & ij (3.3.17)
3.3.2.4 Dot 2 Constraint

The dot2 constraint is that the nonzero vector a j fixed in body j and vector d i j are
orthogonal; i.e.,
Fdot2 (a j , di j ) = a¢jTATj di j = 0 (3.3.18)

Φdot2
qi = a¢jT A Tj [ -I3 -B(pi , s¢i )]
(3.3.19)
Φqdot2
j
= a¢jT A Tj éëI 3 B(p j , s¢j ) ùû + dijT éë0 B(p j , a¢j ) ùû
(F dot2
qi )
cˆ i
qi
= éë0 -a¢jT A Tj B(c pi , s¢i )ùû
(F dot2
qi cˆ )
i q
j
( )
= éë0 - c rTi + χ pTi B T (pi , s¢i ) B(p j , a¢j ) ùû
( )
(3.3.20)
qj c j
F dot2 ˆ = -c pTj BT (p j , a¢j ) [ I 3 B(p i , s¢i )]
qi
(F dot2
qj cˆ )
j
qj
= éëc pTj BT (p j , a¢j ) c ùû
( )
where c = cpTj BT (p j , a¢j )B(p j , s¢j ) + dijT B(c p j , a¢j ) + a¢jT A Tj B( cp j , s¢j ) + crTj + cpTj B T (p j , s¢j ) B(p j , a¢j ) .
Thus,
P2dot2 (qi j , χ ij ) = éë P2idot2 P 2dot2
j
ùû (3.3.21)
where
P2dot2
i = -[cTp j BT (p j , a¢j ) a¢jT ATj B(cpi , s¢i ) + cpTj BT (p j , a¢j )B(pi , s¢i ]
P2dot2
j (
= [cpTj BT (p j , a¢j ) c - crTi + χ pTi BT (pi , s¢i ) B(p j , a¢j )])
And the right side of the acceleration equation is
γ dot2 = P2dot2 (q i j ,&pij )p
& ij (3.3.22)
Expressions for building block constraints and their derivatives derived in this section
have been coded in MATLAB Code 3.11 in Appendix 3.A. They are used to assemble constraint
141
equations of joints defined in subsequent sections and their derivatives for use in kinematic and
dynamic analysis.
3.3.3 Cylindrical, Revolute, and Translational Joints
3.3.3.1 Cylindrical Constraint
The geometry of the joint shown schematically in Fig. 3.3.4 enables relative translation
along and rotation about common body fixed unit vectors v iz and v zj ; i.e., a cylindrical joint. If
constraints are added to prevent relative translation or relative rotation, revolute or translational
joints, respectively, are obtained.
Figure 3.3.4. Geometry of Cylindrical, Revolute, and Translational Joints

The body fixed orthogonal unit vectors v¢xi , v¢yi , and v¢zi shown in Fig. 3.3.4 may be
represented in the global x-y-z frame, to assist in formulation of constraint equations. In terms of
body fixed unit vectors v¢xi , v¢yi , and v¢zi in the x ¢-y¢-z¢ frame shown in Fig. 3.3.4, they are
represented in the x-y-z frame as
v ix = A i v ¢xi
v iy = A i v¢yi (3.3.23)
v iz = A i v¢zi
and likewise for body j. Vectors v¢x , v¢y , and v¢z and their counterparts for body j are constant.
i i i
Points Pi and Pj are fixed in bodies i and j, located as shown in Fig. 3.3.1 by constant body fixed
vectors s¢i and s¢j . Thus, Pi is located in the global frame by the vector r Pi = ri + A is¢i , and P j by
the vector r j = rj + A js¢j .
P
With the vector di j from Pi to Pj of Eq. (3.3.2),
d i j = rj + A js¢j - ri - A i s¢i
the following four cylindrical joint constraint equations specify that vz and vz are collinear:
i j
142
é v xj T di j ù é F dot2 ( v xj , di j ) ù
ê j T ú ê dot2 j ú
ê v y di j ú êF ( v y , di j ) ú
Φ (qi , q j ) º j T i =
cyl
=0 (3.3.24)
ê v x v z ú ê F dot1 ( v iz , v xj ) ú
ê j T i ú ê dot1 i j ú
êë v y v z úû êë F ( v z , v y ) úû
If d i j ¹ 0 these equations imply that vz , di j , and v z are parallel and, since they have points in
i j
common, they are collinear. If d i j = 0 , the third and fourth equations imply that vz and vz are
i j
parallel and, since they have a point in common, they are collinear.
The subJacobians and operators of Eq. (3.1.19) for the cylindrical constraint are obtained
from Eqs. (3.3.14), (3.3.15), (3.3.19), and (3.3.20) for the dot1 and dot2 building block
constraints. Rather than writing explicit expressions for these intricate terms, they are assembled
in MATLAB Code 3.11 of Appendix 3.A, using subroutines that evaluate terms in Eqs. (3.3.14),
(3.3.15), (3.3.19), and (3.3.20).
3.3.3.2 Revolute Constraint
Adding the constraint that the scalar product of Aj v¢z and di j is zero to the cylindrical
j
constraints of Eq. (3.3.24) implies that points Pi and Pj are coincident and yields the revolute joint
that satisfies Eqs (3.3.24) and the fifth revolute joint constraint equation,
Φrev5 (qi , qj ) º v¢zjT ATj di j = Fdot2 (vzj , di j ) = 0 (3.3.25)
The full set of revolute joint constraint equations is

T
Φrev (qi , q j ) º éëΦcylT (qi , q j ) Φ rev5 (qi , q j ) ùû = 0 (3.3.26)
The subJacobians and terms in Eq. (3.1.19) for the revolute constraint are obtained from Eqs.
(3.3.14), (3.3.15), (3.3.19), and (3.3.20) for the dot1 and dot2 building block constraints.
3.3.3.3 Translational Constraint
Adding the constraint that the scalar product of vx and vy is zero to the cylindrical
j i
constraints of Eq. (3.3.24) implies that there can be no relative rotation about the common Ai v¢z
i
axis and yields the translational joint that satisfies Eq. (3.3.24) and the fifth translational joint
constraint equation,
Φtran5 (qi , qj ) º Fdot1 (viy , vxj ) = 0 (3.3.27)
The full set of translational joint constraint equations is

T
Φ tran (qi , q j ) º éëΦcylT (qi , q j ) Φ tran5 (qi , q j ) ùû = 0 (3.3.28)
The subJacobians and operators of Eq. (3.1.19) for the translational constraint are obtained from
Eqs. (3.3.14), (3.3.15), (3.3.19), and (3.3.20) for the dot1 and dot2 building block constraints.
143
Note that the guide in body i and translating bar in body j of Fig. 3.3.4 for the
translational joint need not be cylinders. In reality, and consistent with the present model, they
are rectangular in character.
3.3.4 Composite Joints
Kinematic constraints between a pair of bodies may often be modeled using an
intermediate element, thought of as a massless body, that serves only to define kinematic
relations between the bodies. A substantial library of composite joints for spatial systems is
presented in Vol I of this series (Haug, 1989). Three commonly encountered composite joints are
presented in this section.
3.3.4.1 Universal Constraint
A universal joint between bodies i and j, shown in Fig. 3.3.5, is constructed with an
intermediate body, or cross, that is pivoted in bodies i and j. The geometric definition of the
universal joint is that the center of the cross is fixed in bodies i and j, defined by points Pi and
P j fixed in the respective bodies, and that the axes of the cross are orthogonal. As with most
mechanical joints, a universal joint that connects a pair of bodies can be assembled in two
different ways. For example, body j in Fig. 3.3.5 could be held in the same orientation shown and
the joint could be assembled with body i inverted relative to the cross; i.e., with the vector v ¢zi
pointing upward. In order to define vector equations that characterize this joint, unit vectors
v ¢zi and v ¢z j on bars of the cross in bodies i and j, respectively, as shown in Fig 3.3.5. Since points
Pi and P j must coincide, the spherical constraint equation of Eq. (3.3.8) must hold, where Pi and
P j are located on the respective bodies by vectors si and s j ,
( )
F sph Pi ,Pj º d i j = rj + A js¢j - ri - Ai s¢i = 0 (3.3.29)
Since vectors v ¢zi and v ¢z j lie on the bars of the cross, which must be orthogonal, they must
satisfy the dot1 constraint,
( )
Φdot1 v iz ,v zj = v¢ziT ATi A j v¢zj = 0 (3.3.30)
144
Figure 3.3.5 Geometry of the Universal Joint
In order to show that Eqs. (3.3.29) and (3.3.30) imply the geometry of the universal joint,
note first that Eq. (3.3.29) implies coincidence of points Pi and P j , just as was shown in the case
of the spherical joint. Since vectors v ¢zi and v ¢z j are unit vectors, neither is zero and Eq. (3.3.30)
implies that the two vectors are orthogonal. Thus, the bars of the cross, which intersect at point
P, are orthogonal. These are the geometric conditions for the joint, which are implied by Eqs.
(3.3.29) and (3.3.30).
Note that Eq. (3.3.30) is satisfied if either of the joint definition vectors is reversed in
sign. For example, if v ¢zi is replaced by its negative, which for this joint is equivalent to rotating
the cross about vector v ¢z j by an angle π , the constraint equation is still satisfied; i.e.,
( )
Φdot1 v iz ,vzj = -v iz T v zj = v¢zi T ATi A j v¢z j = 0 . This is consistent with the observation made in geometric
definition of the joint that it can be assembled in two distinct configurations. Since it takes
engineering judgment to determine which configuration is to be selected, it is not surprising that
the mathematical equations that characterize the joint cannot make the distinction between the
two configurations. As is shown in Chapter 2, however, once a configuration is selected, if only
small variations in position and orientation of the bodies are permitted in an increment of time,
enforcement of Eqs. (3.3.29) and (3.3.30) will maintain the configuration of the joint.
Since Eqs. (3.3.29) and (3.3.30) comprise four scalar equations, they remove four relative
degrees of freedom between the bodies connected. This can be seen from Fig. 3.3.5, by holding
body j fixed in space and investigating the admissible motion of body i. Body i is free to rotate
about each of the orthogonal axes of the cross, but cannot rotate about the axis orthogonal to the
cross. It thus has two rotational degrees of freedom, relative to body j.
________________________________________________________________________
Example 3.3.1: Consider the universal joint shown in Fig 3.3.6. The x 1¢ and x ¢2 axes are
constrained to lie in the x-y plane. Let the driving angle of body 1 be θ1 and the output angle of
body 2 be θ 2 . Both bodies are constrained to rotate in the x-y plane, so θ 2 = 0 when θ1 = 0 .
145
Figure 3.3.6 Universal Joint Example
From Fig. 3.3.6,
v¢z1 = [ 0 -1 0]
T
v¢z2 = [ 0 0 -1]
T
é1 0 0 ù
A1 = ê 0 cosθ1 -sinθ1 úú
ê
ëê 0 sinθ1 cosθ1 úû
é cosf -cosq2 sin f sin q2 sin f ù
A 2 = êê sin f cosq2cosf - sin q2 cosf úú
êë 0 sin q2 cosq2 úû
From Eq. (3.3.13),
( )
Φd1 v1z ,v z2 = v¢z1T A1T A 2 v¢z2
é1 0 0 ù é cosf -cosq2sinf sinq2sinf ù é 0 ù
= [ 0 -1 0] ê 0 cosθ1 sinθ1 úú êê sinf cosq2 cosf -sinq2 cosfúú êê 0 úú
ê
ëê 0 -sinθ1 cosθ1 ûú ëê 0 sinq2 cosq2 ûú ëê -1ûú

= -cosθ1sinq2 cosf + sinθ1cosq2 = 0
(3.3.31)
Dividing by cosq1cosq2 ,
tanq1 = tanq 2 cosf (3.3.32)
Hence,
tanq1
q2 = Arctan
cosf
146
Taking the differential of both sides of Eq. (3.3.31),
- ( cosθ1cosq2cosf+ sinθ1sinq2 ) dq2 + ( sinθ1sinq2cosf+ cosθ1cosq2 ) dq1 = 0 (3.3.33)
Similarly, taking the time derivative of Eq. (3.3.31),
- ( cosθ1cosq2cosf+ sinθ1sinq2 ) q& 2 + ( sinθ1sinq2cosf+ cosθ1cosq2 ) q&1 = 0 (3.3.34)
Note that when f = p / 2 , cos f = 0 and for any 0 < q 2 < p / 2 , Eq. (3.3.32) yields tanq1 = 0 ,
which shows that body 1 is locked. Similarly, from Eq. (3.3.33), with cos f = 0 ,
tan q1 tan q 2 dq 2 = dq1 . Since tan q1 = 0 , this equation indicates that for any variation dq 2 ,
dq1 = 0 . This singular behavior is interpreted as lock-up of the driving shaft, as illustrated in Fig.
3.3.7. Thus, the universal joint has a physically singular configuration, which must be avoided in
applications.
Figure 3.3.7 Singular Behavior of Universal Joint

In most applications of the universal joint; e.g., in the driveline of a vehicle, the angle f
in Fig. 3.3.6 is near zero. If f = 0 , cos f =1 and Eq. (3.3.31) shows that q1 = q 2 , so the input and
output angles are the same. Also, Eq. (3.3.34) shows that q& = q& . If f is small, which is the case
2 1
in most applications of the universal joint, Taylor's theorem yields cos f » 1 - f 2 / 2 . Thus, Eq.
(3.3.34) yields
q& 2 » q& 1 +
( sinθ1sinq2 - f2cosθ1cosq2 / 2 )
q& 1 (3.3.35)
( cosθ cosq
1 2 - f sinθ1sinq2 / 2 )
2
and the output velocity varies somewhat from the input velocity. If two universal joints are used
in a driveline, with an intermediate shaft, this variation can be cancelled.
This example clearly shows that there may be singular configurations of mechanisms. It
should however be taken with a grain of salt. The configuration shown in Fig 3.3.6 is typical of
the use of the universal joint, but only when the angle f is small. While the configuration shown
in Fig. 3.3.7 is not practical in an engineering sense, it does illustrate that singularities can occur
in mechanisms if one is careless in design, modeling, or application.
_______________________________________________________________________
147
3.3.4.2 Strut Constraint
The strut composite joint shown in Fig. 3.3.8 consists of a coupler that has a cylindrical
joint about the body fixed vector v ¢z j in body j and a spherical joint at point Pi that is fixed in
body i. This is the so-called McPherson strut suspension shown in Fig. 1.2.6, in the vehicle
model of Section 1.1.5.
Figure 3.3.8 Geometry of the Strut Composite Joint

The geometric definition of the strut composite joint is that vector v zj passes through
point Pi . This requires that v zj and d ij be parallel, hence collinear. Equations intended to imply
this condition are
( )
Φdot2 v xj ,dij = v¢xj A Tj dij = 0
(3.3.36)
Φdot2 ( v ,d ) = v¢ A d
j
y ij y
j T
j ij = 0
To show that Eqs. (3.3.36) imply the geometry of the strut composite joint, note first that
if dij ¹ 0 , these equations imply that v zj and d ij are parallel. Since they have a point in common,
they are collinear and v zj passes through point P j . In the special case dij = 0 , Pi coincides with
P j , and passes through point P j . In either case, Eqs. (3.3.36) imply the geometry of the strut
composite joint.
Since the strut composite joint is defined by two scalar constraint equations, it should
allow four relative degrees of freedom between the bodies connected. To see that this is true,
consider body j as fixed. Point Pi on body i can translate along the u zj axis and body i can rotate
freely in space about point Pi , for a total of four relative degrees of freedom.
3.3.4.3 Revolute-Spherical Constraint
The revolute-spherical composite joint shown in Fig. 3.3.9 is comprised of a massless
coupler with a spherical joint connected to body i at point Pi and a revolute joint connected to
body j, with v zj as its centerline. Joint definition points Pi and Pj define the centers of the
148
respective joints on the bodies. It is required that the vector d ij has constant length equal to
d > 0 and that d ij is orthogonal to v zj .
Figure 3.3.9 Geometry of the Revolute-Spherical Composite Joint

Equations that imply the geometry of the joint are simply the distance constraint between
points Pi and Pj and the dot2 orthogonality condition for vectors v zj and d ij ; i.e.,
( )
Φdist ( Pi ,Pj ,d ) = d Tijdij - d 2 / 2 = 0
(3.3.37)
( )
Φdot2 v zj ,dij = v¢zjT A Tj dij = 0
Since d ¹ 0 , the first equation implies dij ¹ 0 , so the second equation implies v zj and d ij are
orthogonal.
Since two constraint equations characterize this joint, it should allow four relative
degrees of freedom between the bodies connected. If body j is fixed in space, point Pi on body i
is free to move on a one-dimensional circle of radius d in the plane perpendicular to vector v zj . In
addition, body i has three rotational degrees of freedom, for a total of four relative degrees of
freedom.
3.3.5 Kinematic Drivers
In case the distance d defined in Fig. 3.3.5 and Eq. (3.3.1) is specified as a function of
time, d ( t ) > 0 ; e.g., by a hydraulic or electrical actuator, a time dependent distance driver is
defined in the form
ΦdistD ( Pi ,Pj ,d ) º ( dijTdij - d 2 ( t ) ) / 2 = 0 (3.3.38)
This scalar constraint equation eliminates one relative degree of freedom between bodies i and j.
Derivatives of this driver with respect to generalized coordinates and g are the same as for the
distance constraint of Section 3.3.2. Time derivatives for this driver are Φt = -dd& ,
distD
ΦdistD && - dd
= -dd & & , and ΦdistD = 0 .
tt qt
In a cylindrical or revolute joint of Fig. 3.3.4, a relative rotation qi j (t) may be specified as
a function of time, as shown in Fig 3.3.10, where θ i j (q i , q j ) is defined by Eq. (2.4.37). Rather
149
than using this complex relation, Eqs. (2.4.35) and (2.4.36) are more practical. Even these
equations raise intricate issues. Two alternative forms of relative rotation driver may be used,
with the notation of Fig. 3.3.10, as follows:
ìï v¢yiT ATi A jv ¢xj - sin(qi j (t)) = 0, if abs(c) ³ abs(s)
Φ rotD
º í iT T
ïî v¢x A i A j v¢x - cos(qi j (t)) = 0, if abs(s) > abs(c)
j
(3.3.39)
ìï Φdot1 ( v i , v j ) - sin(qi j (t)) = 0, if abs(c) ³ abs(s)
= í dot1 iy xj
ïîΦ ( v x , v x ) - cos( qi j (t)) = 0, if abs(s) > abs(c)
where s = v¢yiT A iT A jv¢xj and c = v¢xiT A Ti A j v¢xj .
Figure 3.3.10 Relative Rotation Driver

SubJacobians of this driver with respect to generalized coordinates are
ìïΦdot1
q i ( v y , v x ),
i j
if abs(c) ³ abs(s)
Φ rotD
= í dot1 i j
ïîΦqi ( v x , v x ), if abs(s) > abs(c)
qi
(3.3.40)
ìïΦdot1
q j ( v y , v x ),
i j
if abs(c) ³ abs(s)
ΦqrotD = í dot1 i j
ïîΦq j ( v x , v x ), if abs(s) > abs(c)
j
and time derivatives for this driver are
ïì-q& (t) cos(qi j (t)), if abs(c) ³ abs(s)

Φ rotD º í & ij
ïî qij (t) sin(qi j (t)), if abs(s) > abs(c)
t
(3.3.41)
ìï-q&& (t) cos(q (t)) + q& 2 (t) sin(q (t)), if abs(c) ³ abs(s)
Φ tt º í && ij
rotD ij ij ij
& 2 (t)cos(q (t)), if abs(s) > abs(c)
q
ïî ij (t) sin( qij (t)) + q ij ij
and Φqt = 0 . Using the subJacobians of Eq. (3.3.40), terms in Eq. (3.1.19) are
rotD
150
(ì Φdot1
ï qi
( v iy , v xj )χˆ qi ) , if abs(c) ³ abs(s)
(Φ rotD
χˆ ) =í
qi
( )
qi qi q
i
ï Φqi ( v x , v x ) χˆ qi
dot1 i j
, if abs(s) > abs(c)
î qi
ì( Φ dot1
( v iy , v xj ) χˆ qi ) , if abs(c) ³ abs(s)
) = ïí
qi
(Φ
qj
rotD
χˆ
ï( Φ )
qi qi q
j dot1
qi ( v ix , v xj )χˆ qi , if abs(s) > abs(c)
î qj
( )
(3.3.42)
ì Φdot1 ( v i , v j )χˆ , if abs(c) ³ abs(s)
(Φ ) ï qj y x qj
qi
rotD
χˆ =í
( )
qj qj
qi
ï Φq j ( v x , v x )χˆ q j
dot1 i j
, if abs(s) > abs(c)
î qi
ì( Φ dot1
( v iy , v xj ) χˆ q j ) , if abs(c) ³ abs(s)
(Φ ) = ïí Φ
qj
qj
rotD
χˆ
ï( )
qj qj
qj dot1
qj ( v ix , v xj ) χˆ q j , if abs(s) > abs(c)
î qj
The P2 operator and right side of the acceleration equation are thus
é
(
P2rotD (q i j , χ i j ) = ê ΦqrotD
ë i
) (
χˆ qi + ΦqrotD
qi j
χˆ q j ) (Φ
qi
qi χˆ
rotD
) + (Φ
qi q
j
χˆ
rotD
qj qj ) ù
qj ú
û (3.3.43)
γ rotD = P2rotD (q i j , q& ij )q& ij
While the expressions for this driver involve two forms of equation and logic to select the proper
form, computer implementation is routine.
In general, when driving constraints are of the form
ΦD (q, t) = Φ(q) + f (t) = 0 (3.3.44)
the associated velocity and acceleration constraint equations are
Φ qD ( q, t)q& + Φ Dt ( q, t) = Φ q ( q )q& + f& (t) = 0 (3.3.45)
q + + γ(q, q)
Φ qD ( q, t)&& & + Φ Dtt (q, t) = Φ q ( q )&& & + &&f (t) = 0
q + γ(q, q) (3.3.46)
Just as subroutines in kinematic analysis computer code are required for evaluation of Φ q (q )
& for all constraints, subroutines are required for evaluation of f (t), f&(t), and &&f (t) for
and γ(q,q)
driving constraints.
3.3.6 Euler Parameter Normalization Constraints
In addition to kinematic constraints and drivers, for spatial systems, Euler parameter
normalization constraints
Φ pi (p i ) = (p Ti p i - 1) / 2 = 0, i=1,L ,nb (3.3.47)
must be satisfied. The Jacobian of this constraint is simply
Φpi (pi )qi = éë0 pTi ùû (3.3.48)
151
and P2 is
(
P 2pi ( p i , χ pi ) = Φ pi ( p i )qi χˆ p i ) qi
(
= p Ti χˆ pi ) qi
= éë0 χ pTi ùû (3.3.49)
Differentiating the velocity equation p Tip& i = 0 , the acceleration equation is p Ti &&

pi = -p& Ti p& i , so the
acceleration right side is
g pi = -p& Tip& i (3.3.50)
3.3.7 Spatial System Examples
3.3.7.1 Two Body Spatial Pendulum
Two spheres of unit radius are shown in Fig. 3.3.12, constrained to form a two-body
spatial pendulum. The origin of the body 1 reference frame is fixed to the origin of the x-y-z
global frame by a spherical joint, so body 1 is free to rotate, but cannot translate. A unit distance
constraint acts between points P1 on body 1 at -u1z ¢ and P2 on body 2 at u¢2z . The spherical joint
between body 1 and ground is simply Φ sph (q ) = r1 = 0 . Instead of retaining r1 as a generalized
coordinate and having this constraint determine it as zero, it is sensible to suppress r1 , delete the
spherical joint between body 1 and ground, and define the vector of generalized coordinates as
T
q = ëép1T r2T p2T ûù Î R11 . Derivatives of constraints in the formulation with respect to r1 are
suppressed.
Figure 3.3.12 Two Body Spatial Pendulum

Data for the distance constraint between bodies 1 and 2 are
dist k = 1: i1 = 1, s1¢P = -u¢z ; j1 = 2, s¢2P = u¢z
With the vector from point P1 to P2 ,
d12 = r2 + A (p 2 )u¢z + A (p1 )u¢z
the distance constraint is
Fdist = ( d12
T
d12 - 1) / 2 = 0
The Euler parameter normalization constraints are
152
Fpi (q) = ( piTpi - 1) / 2 = 0, i = 1, 2
The vector of three kinematic constraints is thus

é( d12
T
d12 - 1) / 2ù
ê ú
Φ(q) = ê ( p1T p1 - 1) / 2 ú = 0
ê ú
ê ( pT2 p 2 - 1) / 2 ú
ë û
Since this is a system of three nonlinear constraint equations in eleven generalized coordinates, if
the constraint Jacobian has full rank, the system should have eight degrees of freedom. These are
three degrees of rotation freedom for each body and two degrees of freedom of point P2 on the
surface of a unit sphere with center at point P1
From Eqs. (3.3.3) and (3.3.48), the constraint Jacobian is
éd12
T
B(p1 , u¢z ) d12
T
d12
T
B(p 2 , u¢z )ù
ê ú
Φq = ê p1T 0 0 ú
ê 0 0 p2T ú
ë û
From Eq. (3.3.4), with χ r1 = 0 , derivatives associated with the distance constraint are
(F dist
q1 1 q
1
χˆ ) = ëé χ Tp1 BT (p1 , u¢z )B(p1 , u¢z ) + d12
T
B( χ p1 , u¢z ) ûù
(F dist
q1 1 q χˆ )
2
= ëé χ pT1 BT (p1 , u¢z ) χ pT1 B T (p1 , u¢z )B(p2 , u¢z ) ùû
(F dist
q2 χˆ ) 2 q
1
( )
= éë χ rT2 + χ pT2 B T (p2 , u¢z ) B(p1 , u¢z ) ùû
(F dist
q2 χˆ ) 2 q
2
= éë( χ T
r2 + χ Tp2 B ( p , u¢ ) ) ( χ
T
2 z
T
r2 )
+ χ pT2 B T (p2 , u¢z ) B(p2 , u¢z ) + d12
T
B( χ p2 , u¢z ) ùû
From Eqs. (3.1.9) and (3.3.49),

éaB(p1 , u¢z ) + d12T
B(χ p1 , u¢z ) a aB(p 2 , u¢z ) + d12 T
B(χ p2 , u¢z ) ù
ê ú
P2(q, χ ) = ê χ p1
T
0 0 ú
ê ú
ë 0 0 χ pT2 û
where
(
a = χpT1 BT (p1 , u¢z ) + χrT2 + χpT2 BT (p2 , u¢z ) )
Finally, from Eq. (3.1.17),
γ = P2(q, q& )q&
3.3.7.2 Spatial Slider-Crank
The spatial slider-crank mechanism shown in Fig. 3.3.13 has been used as a
computational example in dynamics with friction (Marques, Flores, Pimenta Claro, and
153
Lankarani, 2016) and without friction (Haug, 1989). It involves potentially large constraint
reaction forces and near singular configurations that may dominate friction effects. Body 1
rotates about an axis that is parallel to and offset from the global x axis in ground (body 0), as a
crank in the y-z plane of radius of 0.08 m. The slider (body 2) translates in a guide along the
global x axis in ground. A distance constraint of length d3 > 0 connects the centroid of body 2
with the end of the crank in body 1, as a model of the connecting rod. A rotation driver for the
crank about the revolute joint axis is q10 (t) = wt , where w is constant.
Figure 3.3.13 Spatial Slider-Crank

The revolute and translational joints provide five constraint equations each, the distance
constraint provides one, and there are two Euler parameter constraints, for a total of thirteen
nonlinear constraint equations in fourteen generalized coordinates. If the constraint Jacobian has
full rank, the system should have one degree of freedom. Adding the rotational driver should
yield a unique solution as a function of time.
Vectors used in definition of the revolute and translational joints are v¢xik = u z , v¢y = -uy ,
ik
and v¢zik = u x , i = 1 and 2. Other data that define the joints are tabulated as follows:
rev k = 1: i1 = 1, s1¢ = 0, v¢z1 = u x , v¢x1 = u z , v¢y1 = -u y ;

j1 = 0, s¢0 = [0 0.1 0.12]T , v¢z0 = u x , v¢x0 = u z , v¢y0 = -u y
tran k = 2: i 2 = 2, s¢2 = 0, v¢z2 = u x , v¢x2 = u z , v¢y2 = -u y ;
(3.3.51)
j2 = 0, s¢ = 0, v¢ = u x , v¢ = u z , v¢ = -u y
0
2
z
0
x
0
y
0
dist k = 3: i3 = 1, s1¢ = 0.08u z ; j3 = 2, s¢2 = 0, d 3 = dist

rotD k = 4: i 4 = 1; j4 = 0, data same as rev1; q10 (t) = wt
While explicit constraint expressions and their derivatives could be written, as in the
preceding example, it is more convenient and efficient to use computer routines in MATLAB
Code 3.11 of Appendix 3.A to form and solve kinematic equations.
154
3.3.7.3 Automobile Suspension
The automobile suspension outlined in Figs. 1.2.5 to 1.2.7 of Section 1.1 is modeled here
using the schematic of Fig. 3.3.14 and building block and composite constraints. The chassis
(body 1) and front wheel spindle assemblies (bodies 2 and 3) are represented by seven
generalized coordinates each. A revolute-spherical composite joint is pivoted at point A and has
a spherical connection to body 2 at point B. A strut composite constraint acts between body 2 at
point B and body 1 at point C. Identical constraints act on body 3. Front wheel assemblies are
steered with tie rods (distance constraints) that act between the steering rack (body 6), whose
position in the chassis is specified as a function of time, and points on bodies 2 and 3. The right
rear wheel assembly (body 4) is pivoted in a revolute joint with body 1 at point H, using relative
coordinate q14 . The left rear wheel assembly (body 5) is likewise modeled with a relative
coordinate q15 .
Figure 3.3.14 Automobile Suspension Schematic
Constraint connectivity is as follows:

rev-sph k=1: i1 = 1, j1 = 2; 2 constraints
strut k = 2 : i 2 = 2, j2 = 1; 2 constraints
rev-sph k=3: i 3 = 1, j3 = 3; 2 constraints
strut k = 4 : i 4 = 2, j4 = 1; 2 constraints
(3.3.52)
dist k = 5 : i5 = 2, j5 = 6; 1 constraint
dist k = 6 : i6 = 3, j6 = 6; 1 constraint
Eulerparnorm k = 3 : bodies 1,2,3; 3 constraints
Total; 13 constraints
155
T
Generalized coordinates are seven vectors qi = éëriT piT ùû , i = 1, 2,3 , q14 , and q15 , for a total of
23 generalized coordinates. If the constraint Jacobian has full rank, the model has 10 degrees of
freedom. The 10 degrees of freedom are as follows: 6 dof of chassis (body 1) and one wheel
suspension travel degree of freedom for each of four wheel assemblies. Steering angles of the
front wheels are controlled by a specified rack displacement history and the associated tie rod
constraints.
While the spectrum of joints in spatial systems is much broader than that for planar systems, a
library of four building block constraints and associated derivatives is presented as the
foundation for a substantial number of constraints between pairs of bodies. Joints that directly
connect pairs of bodies and joints that involve intermediate couplers, called composite joints,
are presented using the building block constraints. Spatial multibody systems are modeled
using the constraint library for numerical kinematic analysis in Section 3.12.
It is important to note that the library of derivative operators derived in Section 2.6 enables
systematic computer construction of large numbers of nonlinear constraint equations and their
derivatives that are required for kinematic and dynamic simulation. Numerical solution
methods presented in Section 2.2 may thus be brought to bear. Without these systematic tools
and their computer implementation, simulation of the vehicle of Section 3.3.7.3, with its 13
highly nonlinear constraint equations and 23 generalized coordinates, would be unthinkable.
Key Formulas
Fdist ( Pi ,Pj ,d ) º ( dijTdij - d 2 ) / 2 = 0 d ij = rj + A js¢j - ri - A is¢i (3.3.1) (3.3.2)
Fsph ( Pi ,Pj ) º di j = rj + A js¢j - ri - Ai s¢i = 0 (3.3.8)
Fdot1 (ai , a j ) = a¢iT AiTA ja j = 0 Fdot2 (a j , di j ) = a¢jTATj di j = 0 (3.3.13) (3.3.18)
156
3.4 Nonholonomic Constraints
A kinematic constraint that is stated in terms of velocity is called a differential constraint.
If such a constraint is the derivative of an algebraic constraint; i.e., it is integrable, then it
behaves as a holonomic constraint. If it is not integrable, it is called a nonholonomic constraint.
Analytical conditions that determine whether a differential constraint is integrable are given in
Section 2.2.6. No comprehensive approach has been defined for modeling nonholonomic
systems, in contrast to that presented in Sections 3.1 through 3.3 for holonomic systems. Thus,
the focus here is on specific examples and an applicable mathematical formulation. Due to
unique mathematical properties of nonholonomic systems, they will be treated in detail in
Chapter 7.
3.4.1 Examples of Nonholonomic Systems
Three examples are presented with nonholonomic constraints, two with both holonomic
and nonholonomic constraints.
_______________________________________________________________________
Example 3.4.1. The three-wheel vehicle shown in Fig. 3.4.1 is used to transport material in a
manufacturing facility. The radius of the rear wheels and the distance from the floor to point a on
the front wheel assembly are R, so the bed of the transporter, the x¢-y¢ plane, is horizontal. Point
b at the center of the rear axle is the origin of the x¢-y¢-z¢ frame. Generalized coordinates are
f ùû = [ x f] Î R 3 .
T
q = éër T
T
y
Figure 3.4.1 Three Wheel Material Transporter

Since the front wheel is on a trailing arm that pivots about a vertical guide that is parallel
to the z¢ axis, it has no influence on the direction of motion. The rear wheels must roll without
slip, so the velocity of point b must be orthogonal to the axle; i.e., to the x¢ axis. This leads to the
velocity constraint
é x& ù
( A( f)u¢x ) r& = [ cosf sinf] ê ú = 0
T
(3.4.1)
ë y& û
157
In differential form, this is the differential constraint
cosjdx + sinfdy + 0dj = 0 (3.4.2)
Applying the integrability test of Eq. (2.2.44),
sinf( - sinf) + cosf( - cosf) = -1 ¹ 0 (3.4.3)
Thus, the constraint of Eq. (3.4.1) is not integrable; i.e., it is nonholonomic.
It is clear on physical grounds that the transporter can maneuver, using control of rotation
of each of the rear wheels, to any point in the x-y plane, so the nonholonomic constraint does not
impose restrictions on the configuration of the system. If a holonomic constraint were added that
requires both rear wheels roll together, as attached to a rigid axle, then the transporter could
move only along a straight line specified by the initial value of f .
________________________________________________________________________
________________________________________________________________________
Example 3.4.2. A mosquito tracking prey flies near the ground so that its velocity vector is
aimed at a target that moves along the x axis with x-coordinate x (t) , as shown in Fig. 3.4.2. The
mosquito is modeled as a particle that flies in the x-y plane, with a velocity vector that points
toward its prey. The guidance constraint is thus
æ éx(t) ù ö
r& - k ç r - ê ú÷ = 0 (3.4.4)
è ë 0 ûø
where k is a negative constant. Multiplying the first component of Eq. (3.4.4) by y, the second by
x - x(t) , and subtracting the results yields the velocity constraint
yx& - (x - x(t))y& = k ( y(x - x(t)) - (x - x(t))y ) = 0
or, in differential form, the differential constraint

ydx + (x(t) - x)dy + 0dt = 0 (3.4.5)
Unlike examples encountered in Sections 3.1 through 3.3, this constraint is explicitly time
dependent. Applying the integrability test of Eq. (2.2.44) with a = 1, b = 2, and g = 3 ,
0 - (x(t) - x)(0 - 0) + y(0 + x& (t)) = yx& (t) ¹ 0 (3.4.6)

so the constraint of Eq. (3.4.5) is not integrable; i.e., it is nonholonomic.
158
Figure 3.4.2 Mosquito in x-y Plane Tracking Prey
This example has a single nonholonomic constraint and no holonomic constraints. To
illustrate a simple system with both holonomic and nonholonomic constraints, consider a more
sophisticated mosquito shown in Fig. 3.4.3 that flies in a plane that is defined by the holonomic
constraint
Φ(r) º z - y = 0 (3.4.7)
which contains the x axis. The guidance constraint of Eq. (3.4.4) is extended to space as
æ é x(t) ù ö
ç ÷
r& - k ç r - ê 0 ú ÷ = 0 (3.4.8)
ê ú
ç êë 0 úû ÷ø
è
Figure 3.4.3 Mosquito in Plane z = y Tracking Prey

Multiplying the first component of Eq. (3.4.8) by y and the second by x - x( t ) ,
subtracting the results, and converting to differential form yields
ydx - ( x - x ( t ))dy + 0dz + 0dt = 0 (3.4.9)
Applying the integrability test of Eq. (2.2.44) with a = 1, b = 2, and g = 4 , yx& (t) ¹ 0 , so Eq.
(3.4.9) is nonholonomic. Similarly, multiplying the first component of Eq. (3.4.8) by z and the
third by x - x( t ) , subtracting the results, and converting to differential form yields
159
zdx + 0dy - (x - x ( t ))dz + 0dt = 0 (3.4.10)
Applying the integrability test of Eq. (2.2.44) with a = 1, b = 3, and g = 4 , zx& (t) ¹ 0 , so Eq.
(3.4.10) is nonholonomic.
Converting Eqs. (3.4.9) and (3.4.10) to derivative form yields the nonholonomic
constraints
é y - ( x - x( t )) 0 ù
E(r, t )r& º ê r& = 0
-( x - x( t )) úû
(3.4.11)
ëz 0
________________________________________________________________________
________________________________________________________________________
Example 3.4.3. The disk rolling without slip on the x-y plane shown in Fig. 3.4.4 has unit radius.
The plane of the disk is defined by body fixed y¢-z¢ axes and the body fixed x¢ axis is normal to
the plane of the disk. Unit vector a ¢ in the x¢-y¢-z¢ frame from the center of the disk to contact
point C on the periphery with the x-y plane is
é0ù
é0 ù é0 ù
a¢ = êê a 1 úú = ê ú = ê ú a º a¢a a (3.4.12)
a I
êë a 2 úû ë û ë û
and the vector a satisfies the condition
( a a - 1) / 2 = 0
T
(3.4.13)
The normal to the disk periphery at contact point C, in the plane of the disk, is the vector a ¢ , so
the tangent to the disk periphery at point C in the plane of the disk is
é 0 ù
é 0 ù é0ù
b¢ = êê - a 2 úú = ê ú = ê ú a º b¢a a (3.4.14)
Pa P
êë a1 úû ë û ë û
160
Figure 3.4.4. Disk Rolling Without Slip on x-y Plane
Euler parameters are used as generalized coordinates for orientation. Contact conditions
are that point C is in the x-y plane; i.e., u Tz ( r + A(p)a¢aa ) = 0 , and the vector b = A (p )b¢ is in the
x-y plane; i.e., u Tz A(p)b¢aa = 0 . Combining these conditions, Eq. (3.4.13), and the Euler
parameter normalization condition yields the holonomic constraints
éu Tz ( r + A(p)a¢a a ) ù
ê ú
ê u Tz A(p)b¢a a ú
F(q) = =0 (3.4.15)
ê (p Tp - 1) / 2 ú
ê ú
ë (a a - 1) / 2 û
T
T
where generalized coordinates are q = éër T pT a T ùû Î R 9 .
The velocity of point C on the periphery of the disk is

% ¢a¢aa = r& + A(p)A T (p)A(p)a
v C = r& + A(p)ω & ¢a a
(3.4.16)
&
= r& + A(p)a¢a a = r& + ( A(p)a¢a aˆ )p p& = r& + B(p,a¢a a)p&
Conditions that no slip occurs between the disk and the x-y plane are that the horizontal
components of v C are zero; i.e., u Tx v c = 0 and u Ty v c = 0 . With Eq. (3.4.16), this is
éuTx u Tx B(p,a¢aa) 0 ù
E(q)q& º ê T ú q& º e(q, t) = 0 (3.4.17)
ëêu y u Ty B(p,a¢aa) 0 ûú
With four holonomic and two differential constraints on nine generalized coordinates, the
system has five configuration and three velocity degrees of freedom. As in this example, most
mechanical systems that are subjected to differential constraints are also subjected to holonomic
constraints. In such situations, it is impractical to determine whether the differential constraint is
integrable, so it is treated as if it is nonholonomic.
_______________________________________________________________________
161
3.4.2 System Holonomic and Nonholonomic Constraints
Planar and spatial holonomic constraints between bodies presented in Sections 3.1
through 3.3 and differential, or nonholonomic, constraints such as those illustrated in Section
3.4.1 are assembled for system position, velocity, and acceleration analysis. This process is
largely an exercise in bookkeeping, which must be carried out meticulously to support digital
computer formulation and solution of the nonlinear equations of kinematics and dynamics.
T
With system generalized coordinates q = éëq1T L q Tnb ùû for nb bodies that make up a
system, holonomic constraints that include planar constraints of Section 3.1, spatial constraints
of Section 3.2, and Euler parameter normalization conditions for spatial systems are written in
the form
F (q, t ) = [Φ1 (q, t ) L Φ nhc (q, t ) ] = 0

T
(3.4.18)
for a total of nhc possibly time dependent holonomic constraints on the system configuration.
These constraint equations are properly posed only if the Jacobian Φq (q,t) has full row rank.
A total of nd differential constraints that act on the system, such as those in Section
3.4.1, are of the form
E(q, t )q& = e(q, t ) (3.4.19)
where E(q, t ) is an nd ´ ngc matrix of rank nd.
Combining the holonomic velocity equations that are obtained by differentiating Eq.
(3.4.18) with respect to time,
F q (q, t)q& = -F t (q, t) º n h (q, t) (3.4.20)
with the differential constraints of Eq. (3.4.19) yields a total of nv = nhc + nd ³ nhc system
velocity constraint equations,
éFq (q, t ) ù é ν h (q, t )ù
C(q, t )q& º ê ú q& = ê ú º ν(q, t ) (3.4.21)
ë E(q, t ) û ë e(q, t ) û
These equations are properly posed only if C(q, t ) has full rank.
Differentiating Eqs. (3.4.20) with respect to time yields acceleration equations due to
holonomic constraints,
F q (q, t)q ( )
&& = - F q (q, t)qˆ& q& - 2F tq (q, t)q& - F t (q, t) º - γ h (q, q& ,t)
q
(3.4.22)
and differentiating Eq. (3.4.19) with respect to time yields the acceleration equations due to
differential constraints,
E(q, t)q ( )
&& = eq (q, t)q& + e t (q, t) - E(q, t)qˆ& q& - E t (q, t)q& º -γ nh
q
(3.4.23)
Combining Eqs. (3.4.22) and (3.4.23) yields nv system acceleration constraint equations,
162
é -γ (q,q, & t) ù
&& = ê h
C(q, t )q º -γ(q,q,
& t)
& t )úû
(3.4.24)
ë -γ nh (q,q,
These equations are likewise properly posed only if C(q, t ) of Eq. (3.4.21) has full rank.
As presented in Section 3.1, holonomic constraint k due to a joint that acts between
bodies i and j in the system, including the possibility of a time dependent kinematic driving
constraint, is represented by algebraic relations between q i and q j of the form
F k (qi , q j , t) = 0 (3.4.25)
(
k = 1,L , nh , and include Euler parameter normalization conditions pTi pi - 1 / 2 = 0 for spatial )
bodies. The associated kinematic velocity equations
Fqki&qi + F qk j&q j = -F t (3.4.26)
and virtual displacement equations

Fqki δqi + Fqkj δq j = 0 (3.4.27)
must hold.
Similarly, differential constraints between bodies i and j are
Eik (qi , q j , t)q& i + Ekj (qi , q j , t)q& j = e k (qi , q j , t) (3.4.28)
k = nh + 1L nh + nd . The associated virtual displacement equations are

Eik δqi + Ekj δq j = 0 (3.4.29)
The subJacobian for constraint k of Eq. (3.4.25) or (3.4.28), relative to body i, is

ìïFqki or Eik , if constraint k involves body i
C ºí
k
i , i = 1L nb (3.4.30)
ïî0 , otherwise
The full Jacobian for constraint k that includes contributions from all bodies is
Ck º éëC1k C2k L Cnb
k
ùû (3.4.31)
where, as defined in Eq. (3.4.30) only Cαk and Cβk are nonzero for bodies α and β that are
connected by constraint k. The system constraint Jacobian is
T
C º éëC1T C2T L CncT ùû (3.4.32)
For systems that involve many bodies and constraints, the system Jacobian is highly sparse; i.e.,
most of its entries are zero.
163
In contrast to holonomic constraints encountered in Sections 3.1 through 3.3, nonholonomic
constraints on and between bodies that are written in terms of velocities of the bodies tend to
be ad-hoc in nature. Three examples illustrate the form of nonholonomic constraint equations,
which are linear in velocities and nonlinear in configuration generalized coordinates. Two of
the examples include both holonomic and nonholonomic constraints.
System holonomic and nonholonomic constraint Jacobians are generally sparse, enhancing the
efficiency of numerical solution. Linear system velocity and acceleration equations that include
both holonomic and nonholonomic constraint velocity and acceleration equations are properly
posed only if the system Jacobian has full rank.
Key Formulas
F (q, t ) = [Φ1 (q, t ) L Φnhc (q, t )] = 0

T
q = éëq1T qT2 L q Tnb ùû Î R ngc
T
(3.4.18)
E(q, t )q& = e(q, t ) F q (q, t)q& = -F t (q, t) º n h (q, t) (3.4.19) (3.4.20)
éF q (q, t ) ù é ν h (q, t )ù
C(q, t )q& º ê ú q& = ê ú º ν(q, t ) (3.4.21)
ë E(q, t ) û ë e(q, t ) û
é -γ (q,q, & t) ù
&& = ê h
C(q, t )q º - γ(q,q,
& t)
& t ) úû
(3.4.24)
ë - γ nh (q,q,
164
3.5 Configuration Space and the Constraint Manifold
The multibody planar and spatial constraint formulations of Sections 3.2 and 3.3 provide
a uniform means of modeling a broad spectrum of mechanical systems. The models, however,
are highly nonlinear and kinematic constraint equations must be computer generated, precluding
any possibility of analytical solution. This dilemma is addressed in this and the following
sections, using a practical implementation of mathematical concepts of differential geometry on
the Euclidean space R ngc in which mechanical systems function.
3.5.1 Configuration Space
Configurations q Î R ngc of a mechanical system that satisfy nhc holonomic constraint
equations of Eq. (3.1.22),
F(q) = 0 Î R nhc (3.5.1)
comprise the configuration space of the system,
C º {q : F (q ) = 0} Ì R ngc (3.5.2)
This is a subset of Euclidean space R ngc that defines all configurations of the system that are
consistent with the constraints of Eq. (3.5.1). If the constraints are independent, to be defined
below, the system has DOF = ngc - nhc degrees of freedom.
With the induced topology defined in Section 2.2.4, the configuration space C is a
topological space, so continuity of functions of points in C is defined. Since the function Φ(q ) is
generally nonlinear, C is generally not a vector space. To see that this is the case, let q1 and q 2
belong to C; i.e., Φ(q1 ) = 0 = Φ(q2 ) . Due to nonlinearity of the function Φ(q ) , however, it is
expected that Φ(q1 + q2 ) ¹ Φ(q1 ) + Φ(q2 ) = 0 and q1 + q2 is not in C.
Based on Cartesian coordinate kinematics of Sections 3.2 and 3.3, four systems used in
illustrations have the numbers of bodies (nb), generalized coordinates (ngc), holonomic
constraints (nhc), and degrees of freedom (DOF) shown in Table 3.5.1. The dimension ngc of
configuration space C of Eq. (3.5.2) for each of these systems clearly precludes drawing pictures
in R ngc to gain insight into kinematics of even the most modest of these systems. This is
motivation for the differential geometric approach introduced in this and the following section
and used throughout the remainder of the text.
Table 3.5.1 Dimension of Coordinates, Constraints, and Degrees of Freedom
System Section nb ngc nhc DOF
Planar Slider-Crank 3.2.3.2 3 9 8 1
Planar Windshield Wiper 3.2.3.4 3 9 8 1
Spatial Slider-Crank 3.3.7.2 2 14 13 1
Spatial Automobile Suspension 3.3.7.3 6 23 13 10
As defined in Section 2.2.4, endowed with open sets of the form W Ç C , where W is
open in R ngc , C is a topological space. This simple construction on subset C of Euclidean space
165
R ngc , using open sets that are unions of balls Be (q0 ) defined in Eq. (2.2.47), enables
parameterizations; i.e., changes of variable, that support modern analytical and computational
methods in mechanical system kinematics and dynamics. The purpose of this and the following
section is to provide a self-contained introduction to differential geometry in Euclidean
configuration space R ngc that underpins mechanical system kinematics and dynamics. This is in
contrast to mathematical literature on differential geometry and differential topology that
consists of hundreds of thousands of pages focused on abstract spaces and topologies that may or
may not provide practical tools to deal with mechanical system kinematics and dynamics in
Euclidean space. To identify issues that must be dealt with in this development a simple model
of a slider-crank mechanism is first considered.
_____________________________________________________________________________
Example 3.5.1 The two-body slider-crank model shown in Fig. 3.5.1 is represented by two
generalized coordinates q = [q1 q 2 ] that are subject to the single holonomic constraint
T
Φ(q ) = ( (cosq1 - q 2 ) 2 + sin 2 q1 - 1) / 2 = 0 (3.5.3)
The configuration space of Eq. (3.5.2) is the union of four subsets

C = A È BÈ D È E (3.5.4)
where
A º {q Î R 2 : - p / 2 £ q1 £ p / 2, q 2 = 0}
B º {(q Î R 2 : p / 2 £ q1 £ 3p / 2, q 2 = 0}
D º {q Î R 2 : - p / 2 £ q1 £ p / 2, q 2 = 2cosq1}
E º {q Î R 2 : p / 2 £ q1 £ 3p / 2, q 2 = 2cosq1}
Configurations in each subset are shown in Fig. 3.5.2.
Figure 3.5.1 Slider-Crank Mechanism
Figure 3.5.2 Configurations of Slider-Crank
166
Note that at each of the isolated configurations q = [ ±p / 2 0 ] , four distinct paths in C
T
may be followed. Two paths involve having the slider locked at q 2 = 0 and q1 departing ±p / 2
in two directions, as in subsets A and B, and two paths involve positive and negative
displacements of the slider and associated angles q1 ¹ ±p / 2 , as in subsets D and E. The
bifurcations that may occur at q1 = ±p / 2 are problematic. To see what is happening, note that
the constraint Jacobian
Φq = [q 2 sin q1 -(cosq1 - q 2 )] (3.5.5)
is zero at configurations q = [ ±p / 2 0 ] . The implicit function theorem thus suggests that

T
neither of the generalized coordinates can be uniquely determined as a function of the other in
neighborhoods of these configurations. These are singular configurations. As with the
fundamental difficulty identified in Example 1.2.1 in finding two generalized coordinates to map
the entire unit sphere, this simple example is a warning that singular behavior may be
encountered in realistic mechanical systems.
This mechanical system configuration space contains singularities and subsets on which
singularity free motion of the system takes place. Subsets of C on which the constraint Jacobian
has full rank are called regular, in contrast to the singular behavior at q = [ ±p / 2 0 ] , where
T
the Jacobian fails to have full rank. Generalized coordinates in the configuration space for which
the constraint Jacobian has full row rank comprise the regular configuration space,
C% = {q Î C : rank(Φ q (q )) = nhc} Ì R ngc (3.5.6)
Defining disjoint subsets of C% as

% º {q Î R 2 : - p / 2 < q < p / 2, q = 0}
A 1 2
% º {q Î R 2 : p / 2 < q < 3p / 2, q = 0}
B 1 2
% º {q Î R 2 : - p / 2 < q < p / 2, q = 2cosq }

D 1 2 1
E% º {q Î R 2 : p / 2 < q1 < 3p / 2, q 2 = 2cosq1}
C% = A
% ÈB
% ÈD
% È E% .
With the induced topology defined in Section 2.2.4 by open sets of the form W Ç C% ,
where W is open in R ngc , C% is a topological space that is free of singular aspects of the full
configuration space C. Since generalized coordinates in C% satisfy conditions for C and the
additional condition that rank(Φq (q)) = nhc , C% Ì C . If there are generalized coordinates q Î C
for which rank (Φ (q )) < nhc , then C% ¹ C and configurations that satisfy the constraints, but for
q
which the constraint Jacobian fails to have full rank, display singular behavior. This behavior is
shown in greater generality in Sections 3.6 and 3.7.
______________________________________________________________________________
167
The condition rank(Fq (q )) = nhc is equivalent to the inequality Fq ( q )FqT (q ) ¹ 0 . Thus
the complement of C% in C is the closed set in which Fq ( q )FqT (q ) = 0 and C% is open in C. If
there are no singular configurations, C% = C and C% is both open and closed in C.
3.5.2 Constraint Manifolds
Rather than giving an abstract definition of manifolds, as is done in the mathematical
literature (Spivak, 1999; Lee, 2013; Dundas, 2018), it is instructive to introduce the concept
using properties of nonlinear equations of kinematics derived in Sections 3.1 through 3.3. A
family of parameterizations of subsets of the regular configuration space C% of Eq. (3.5.6) is
defined by invertible mappings from R ngc - nhc to C% , that locally represent C% . Such
parameterizations are first introduced, using Example 1.2.1 of Section 1.2.2 for a particle that is
constrained to move on a unit sphere. The concept is further developed for higher dimensional
mechanical systems in this and the following section.
________________________________________________________________________
Example 3.5.2: Consider the particle on a unit sphere shown in Fig. 3.5.3 that was introduced in
Example 1.2.1 of Section 1.2.2. Vectors q to points on the unit sphere satisfy the holonomic
kinematic constraint,
Φ(q) = (qTq - 1) / 2 = 0 (3.5.7)
Points on the unit sphere in R 3 define the configuration space of Eq. (3.5.2),
C = {q Î R 3 : qT q = 1} (3.5.8)
Since Fq (q) = qT ¹ 0 for q Î C , the constraint Jacobian has full rank throughout C and the
regular configuration space is
C% = {q Î C : Φq (q ) = qT ¹ 0} = C (3.5.9)

At a point q 0 on the unit sphere, the vector q 0 is orthogonal to the surface of the sphere
and vectors w tangent to the surface satisfy F q (q 0 )w = q0T w = 0 . Since q 0 ¹ 0 cannot coincide
with all three of the unit coordinate vectors u x , u y , and u z , say q0 ¹ aux , define the unit vector
168
( )
V1 = 1 / u% x q 0 u% x q 0 that is orthogonal to u x and q0 . Next, define the unit vector
V2 = (1 / q% V ) q% V that is orthogonal to q
0 1 0 1 0
and V1 . Since the unit vectors V1 and V 2 are
orthogonal and both are orthogonal to q 0 , they form a basis for the tangent plane, or tangent
space, to the sphere at point q 0 . Define
U = ΦTq (q0 ) = q0 (3.5.10)
and
V = éë V1 V 2 ùû (3.5.11)
where, by construction,
VTV = I
VTU = 0 (3.5.12)
UTU = 1
Since the unit vector U is orthogonal to the two linearly independent columns of V, the columns
of V and U form a basis for R 3 .
Since Euclidean space R 3 is a vector space of dimension 3, any vector a Î R 3 can be
uniquely represented as a = Vv - Uu . Multiplying both sides of this equation by VT and UT ,
v = VTa and u = -U Ta , so a = VV Ta + UU Ta . Thus, for all a Î R 3 , ( I - VV T - UU T ) a = 0 and
U and V are related by the condition
I = VV T + UU T (3.5.13)
Every vector q in R 3 can be uniquely written in terms of tangent space generalized
coordinates v Î R 2 and u Î R1 , defined as
q = q0 + Vv - uU (3.5.14)
where the sign of the third term on the right is selected to represent a projection onto the regular
configuration space from the tangent space, as shown schematically in Fig. 3.5.4. It is important
to note that this representation of configurations is possible because Euclidean space R 3 is a
vector space of dimension 3.
Figure 3.5.4 Orthogonal Projection onto Regular Configuration Space
169
Multiplying Eq. (3.5.14) on the left by V T and UT and using the relations of Eqs. (3.5.12)
yields the inverse relations
v = V T (q - q0 )
(3.5.15)
u = - U T (q - q 0 )
In particular, Eq. (3.5.15) implies v(t 0 ) = v 0 = 0 and u(t 0 ) = u 0 = 0 .

Requiring that the configuration q of Eq. (3.5.14) satisfies Eq. (3.5.7) yields
Φ( q 0 + Vv - uU ) = ( u 2 - 2u + v T v ) / 2 = 0 (3.5.16)
Using the quadratic formula, u = 1 ± 1 - v T v . Since u = 0 if v = 0 , the minus sign must be

selected, so
u = 1 - 1 - vT v º h( v) (3.5.17)
and Eq. (3.5.14) is
( )
q º ψ(v) = q0 + Vv - 1 - 1 - v T v q0 = q0 + Vv - Uh(v) (3.5.18)
Clearly, this parameterization is only valid if v T v < 1 . Geometrically, this means that if v T v > 1 ,
the vector perpendicular to the tangent space through the point q0 + Vv in Fig. 3.5.4 does not
intersect the unit sphere in R 3 .
Substituting q of Eq. (3.5.18) into Eq. (3.5.7),
Φ(q ) = (q Tq - 1) / 2 = ( v T V T Vv + (1 - v T v )q 0Tq 0 - 1) / 2 = ( v T v - v T v ) / 2 = 0
which verifies that the holonomic constraint of Eq. (3.5.7) is satisfied for arbitrary v in a
neighborhood V0 of v 0 = 0 . The representation of Eq. (3.5.18) is thus a local parameterization
% of Eq. (3.5.9).
of the regular configuration space C
To see that the mapping ψ is locally one to one, set y(v1 ) = y(v2 ) for v1 and v 2 in V0 ,
y(v1 ) = q0 + Vv1 - Uh(v1 ) = q0 + Vv 2 - Uh(v 2 ) = y(v 2 )

Multiplying both sides by V T and using Eqs. (3.5.12), v1 = v 2 . Thus, the mapping is locally one
to one; i.e., one to one in the neighborhood V0 .
Multiplying both sides of Eq. (3.5.18) by V T yields
v = V T ( q - q0 ) = V Tq º f(q) (3.5.19)
where Eqs. (3.5.12) have been used. From Eqs. (3.5.18) and (3.5.19),
( ( ) )
f(y(v )) = V T q 0 + Vv - U 1 - 1 - v 2 - q0 = v , for all v Î V0 ; i.e. f is the inverse of ψ ,
f-1 = ψ (3.5.20)
170
The mappings y and f are continuous in the induced topology; i.e., the metric topology on R 3 ,
so they are homeomorphisms (Lee, 2010; Schlichtkrull, 2013) that define C% as approximately
Euclidean in a neighborhood of v = 0 . An important property of these mappings is that
y : R2 ® C % Ì R 3 maps open sets of R 2 onto open sets of C% , which are the intersection of open
sets in R 3 with C% , so C% is an open set of dimension 2. This defines the number of degrees of
freedom of the system in ψ(V0 ) .
To more explicitly verify that q of Eq. (3.5.18), for v of Eq. (3.5.19), satisfies Eq.
(3.5.18), using Eq. (3.5.19),
v T v = ( q T V ) V T q = q T ( VV T ) q = q T ( I - UU T ) q = q Tq - ( q 0Tq ) = 1 - ( q 0Tq )
2 2
Substituting this result into the right side of Eq. (3.5.18) and using Eq. (3.5.13) yields the desired
æ 2 ö
result, y(f(q) ) = q0 + VV Tq - q0 ç 1 - 1 - 1 + ( q0Tq ) ÷ = ( VV T + UU T ) q = q . Thus,
è ø
ψ -1 = f (3.5.21)
With domain U 0 = ψ ( V 0 ) , f is a differentiable map from an open set U0 Ì C

% onto an
open set V0 Ì R 2 , with continuous inverse ψ . In the language of differential geometry (Spivak,
1999), or differential topology (Lee, 2010; Schlichtkrull, 2013), the pair ( f,U 0 ) is a chart on C% .
With a family of configurations qi Î C % , the associated neighborhoods U i = y(Vi ) Ì C%

define a family of charts ( fi , U i ) on C% . If enough configurations q i and associated charts are
defined so that C% = Èi U i , the family of charts is called an atlas for C% . This terminology is
motivated by a family of maps of the surface of the earth that are planar representations of
regions U i on the surface of the earth such that the maps agree on overlapping regions. These
maps are then bound into a book (atlas) that represents a large region; e.g., a country.
The mapping of Eq. (3.5.18) is a parameterization of the nonlinear regular configuration
space C% in terms of the variable v on open subsets V 0 of R 2 . This provides a local
mathematical representation of kinematics of the system on the linear space of real variables R 2 .
The essential nonlinearities of the kinematic system are imbedded in the function ψ(v ) . It is
important to note that C% cannot be parameterized by a single mapping from R 2 . It is only fully
represented if enough charts (fi ,U i ) are defined such that fi (v) are local parameterizations of C%
on U i and C% = È U i . The reason multiple charts are required for the unit sphere is explained
i
later, when a bit more machinery is available.
Since the transformation of Eq. (3.5.18) is valid for v T v < 1 , with the restriction
v T v £ 0.8 , the chart domains U i comprise approximately 80% of the area of the hemisphere with
q0 as pole. Therefore, the union of a small number of such charts covers the unit sphere.
_______________________________________________________________________
171
While all the machinery used in Example 3.5.1 is not required to analyze the kinematics
of a particle on the unit sphere in R 3 , it is presented as an introduction to concepts of differential
geometry that are required for analysis of the kinematics and dynamics of broad classes of
mechanical systems in Euclidean space R n , n > 3, where drawings of surfaces are not possible.
Fortunately, a rich body of theory of differential geometry that has potential to positively impact
the theory and practice of kinematics and dynamics of mechanical systems has emerged in the
mathematical literature. Unfortunately, much of the mathematical literature is clothed in highly
abstract mathematical garb that makes it difficult to access by we mere mortal engineers. For this
reason, a subset of this theory that is applicable for kinematics and dynamics of mechanical
systems in Euclidean space R ngc that is developed in (Schlichtkrull, 2013) is summarized in this
and the following section.
As an illustration of this theory, no chart in Example 3.5.1 can parameterize the entire
surface of the unit sphere. The reason is that the unit sphere is closed and bounded in the
topology induced by R 3 , hence compact. Any single homeomorphism that would map it onto a
set in R 2 would require that the image would be compact (Mendelson, 1962). However, the
image of a chart in R 2 is open, yielding a contradiction. Thus, no single chart can parameterize
the entire unit sphere. This is the mathematical reason for the dilemma encountered in Example
1.2.1 and is analogous to the result of (Stuelpnagel, 1964), cited in Section 2.4.3, that there exists
no singularity free set of three orientation generalized coordinates for a body in space. It is these
theoretical results that require use of local parameterizations; i.e., charts, to characterize
constraint manifolds. Unfortunately, this reality has not yet permeated the literature of
3.5.3 Tangent Space Parameterization of Constraint Manifolds
The geometry of Example 3.5.1 may be generalized, by observing that kinematically
admissible velocities q& of Eq. (3.1.23); i.e.,
Fq (q)q& = 0 (3.5.22)
% are by definition tangent to the regular configuration space at q. Geometrically, this
at q Î C
says that the tangent space of regular configuration space at q is comprised of vectors in the null
space N q = {y Î R n : Φ q (q )y = 0} of the constraint Jacobian Φq (q ) at configuration q. Hence,
vectors that are orthogonal to the tangent space at point q Î C % are linear combinations of
columns of the transpose Φ Tq (q ) of the constraint Jacobian Φq (q ) . Note that this requires that
the Jacobian has full rank, as is embedded in the definition of C% .
% defines an ngc ´ nhc matrix U
The constraint Jacobian F q (q ) at a configuration q0 Î C
whose column vectors are orthogonal to the tangent space of C% at q 0 ; i.e.,
U º F0T
q = Fq (q )
T 0
(3.5.23)
172
% , the constraint Jacobian has full row rank, so the columns of U are linearly
For q 0 Î C
independent and U T U is positive definite, hence nonsingular. An ngc ´ (ngc - nhc) matrix V in
the null space of F q (q 0 ) is defined as the solution of
Φ 0q V = U T V = 0
(3.5.24)
VTV = I
using singular value decomposition (Strang, 1980). The second of Eqs. (3.5.24) implies that V
has full rank. Since its columns are orthogonal to the columns of U , the columns of U and V
span the ngc-dimensional Euclidean vector space R ngc .
Matrices U and V are further related. Any vector a Î R ngc is uniquely represented as
a = Vv - Uu (3.5.25)
where v Î R ngc- nhc and u Î R nhc . Multiplying on the left by V T and using the second of Eqs.
(3.5.24), v = V Ta . Similarly, multiplying on the left by U T and using the first of Eqs. (3.5.24),
u = -( U T U )-1 U Ta . Substituting these relations into Eq. (3.5.25), a = VV Ta + U ( U T U ) -1 U T a , or
( VV T
+ U ( U T U )-1 U T - I ) a = 0 , for arbitrary a. Thus,
VV T + U( U T U )-1 U T = I (3.5.26)
A tangent space parameterization of C% in a neighborhood U 0 of q 0 is defined as
q = q0 + Vv - Uu (3.5.27)
The variables v and u comprise a set of local generalized coordinates that are in one to one
correspondence with q Î U 0 . To see this, multiply Eq. (3.5.27) by V T and U T and use Eqs.
(3.5.24) to obtain unique values of v and u,
v = V T (q - q 0 )
(3.5.28)
u = - ( U T U ) U T (q - q 0 )
-1
There is thus a one to one correspondence between q and (u,v) in a neighborhood of q 0 . Further,
from Eq. (3.5.28) at q = q 0 ,
v0 = 0
(3.5.29)
u0 = 0
To assure that q of Eq. (3.5.27) satisfies the holonomic constraints of Eq. (3.5.1), it is
required that
Φ ( q0 + Vv - Uu ) = 0 (3.5.30)
The derivative of the left side of this equation with respect to u, with v held constant, is
( ( ˆ ˆ - Uu
Φ qˆ 0 + Vv ˆ ))
= -Φ (q)U , or
u
q
173
Φ u ( q 0 ) = -Φq (q 0 ) U = - U T U º -( B0 ) -1 (3.5.31)
which is nonsingular, since the Jacobian has full rank at q 0 . Since Fq (q) is a continuously
differentiable function of q, Fq (q)U = Fq (q)FqT (q0 ) is nonsingular in a neighborhood U 0 of
q0 and
B( q ) º ( Φ q ( q )U )
-1
(3.5.32)
is continuously differentiable with respect to q in U0 .

Since Φ u (q ) = -Φ q (q )U = - B -1 (q ) is nonsingular in a neighborhood of q 0 and u = 0 is
a solution of Eq. (3.5.30) with v = 0 , the implicit function theorem of Section 2.2.5 guarantees
that Eq. (3.5.30) has a unique continuously differentiable solution
u = h( v) (3.5.33)
in a neighborhood V 0 of v = 0 . Thus, Eq. (3.5.27) becomes

q º ψ(v ) = q0 + Vv - Uh( v ) (3.5.34)
which satisfies Eq. (3.5.1), for all v in V 0 and y : U0 ® ψ(V0 ) º U 0 Ì C % . It is important to note
that the right side of Eq. (3.5.27) is linear in v and u, but the right side of Eq. (3.5.34) is a
nonlinear function of v. This is because the nonlinear function h(v) is defined to satisfy the
nonlinear constraint equation of Eq. (3.5.1). The nonlinearity of the constraint equations is thus
embedded in the function h(v).
To see geometrically what Eq. (3.5.34) means and why the extent of the neighborhood
V is important, the geometry of choosing u to stay in C% of Eq. (3.5.6) is shown schematically
0
in Fig. 3.5.5. The vector q0 + Vv may be viewed as movement in the tangent plane of C% , which
must be modified by vector -Uu to yield q( v ) on C% . Equation (3.5.34) consolidates the vectors
into the continuously differentiable function q = ψ( v) of Eq. (3.5.34). It is clear from Fig. 3.5.3,
however, that if v is large, the vector -Uu that is perpendicular to the tangent space to C% at q0
may not intersect C% , in which case the parameterization fails; i.e., v is no longer a valid vector of
generalized coordinates. Fortunately, a large condition number of B( q ) , as defined in Section
2.2.8, is an effective warning that such a situation is eminent, in which case a new vector q 0 is
defined as the current value of q, new bases U and V are computed, and analysis is continued
with a new mapping y ( v ) .
Figure 3.5.5 Projection onto C%
174
3.5.4 The Regular Configuration Space C% is a Differentiable Manifold
Note that Fig. 3.5.5 is drawn in a two-dimensional plane, even though the vectors
involved reside in R ngc . The beauty of differential geometry is that its analytical constructs may
be viewed as abstractions of geometric concepts in R 2 and R 3 . It is important, however, that care
be taken to be rigorous in the analytical setting, lest what may seem to be obvious in R 2 or R 3
does not generalize to high dimensional spaces.
To see that the mapping y : V0 ® U0 of Eq. (3.5.34) is one to one, for v1 and v 2 in V0 ,
set
y(v1 ) = q 0 + Vv1 - Uh(v1 ) = q0 + Vv2 - Uh(v 2 ) = y(v2 )
Multiplying both sides by V T and using Eq. (3.5.24), VTq0 + v1 = VTq0 + v2 , or v1 = v 2 , so the
mapping y is one-to-one on V0 .
Multiplication of Eq. (3.5.34) on the left by V T yields the mapping

v º f(q ) = V T ( q - q0 ) (3.5.35)
To see that f-1 is y ,
f(y(v )) = V T ( y(v ) - q 0 ) = V T ( q 0 + Vv - Uh( v ) - q0 ) = v (3.5.36)
for all v Î V0 ; i.e.,

f-1 = y (3.5.37)
This shows that y is a homeomorphism (Lee, 2010; Schlichtkrull, 2013) that maps open sets of
% Ì R ngc . Thus, C% is an ngc - nhc dimensional subset of R ngc that is
R ngc - nhc onto open sets of C
% is open in C% .
open in the induced topology of C% Ì R ngc ; i.e., for N open in R ngc , N I C
To show that y -1 = f , using Eqs. (3.5.34) and (3.5.35),
y( f(q )) = q0 + VV T (q - q 0 ) - Uh ( V T (q - q 0 ) )
(3.5.38)
= ( I - VV T ) q0 + VV Tq - Uh ( V T (q - q 0 ) )
Since h ( V T (q - q0 ) ) is the unique solution of Eq. (3.5.30) with v = VT (q - q0 ) for q Î U0 ,
(
Φ q0 + VV T (q - q 0 ) - Uh ( V T (q - q0 ) ) ) (3.5.39)
( )
= Φ ( I - VV T ) q 0 + VV Tq - Uh ( V T (q - q0 ) ) = 0
Since q of Eq. (3.5.34) satisfies Φ ( q ) = 0 , a solution for h ( V T (q - q0 ) ) is obtained by setting

the argument of Eq. (3.5.39) equal to q; i.e.,
q = ( I - VV T ) q0 + VV Tq - Uh ( V T ( q - q0 ) )
175
Multiplying both sides of this relation by U T and using the relation U T V = 0 ,
h ( V T (q - q 0 ) ) = ( U T U ) U T {( I - VV ) q }
+ VV T q - q = - ( U T U ) U T ( q - q 0 )
-1 T 0 -1
This is the unique solution of Eq. (3.5.39). Substituting this result into Eq. (3.5.38), and using
Eq. (3.5.26),
( ) (
y( f(q )) = I - VV T - U ( U T U ) U T q0 + VV T + U ( U T U ) U T q = q (3.5.40)
-1 -1
)
which shows that
y -1 = f (3.5.41)
With domain U 0 = ψ ( V 0 ) , f : U 0 ® V 0 is a continuous map from an open set U0 Ì C

%
onto an open set V0 Ì R nhc , with continuous inverse ψ . A countable collection of charts
( fi , U i ) , i = 1, 2,L , such that the union of the Ui covers C% ; i.e., C% = È U i is called an atlas
i
and provides a basis for characterizing C% as a diffeerentiable manifold (Lee, 2010;

Schlichtkrull, 2013).
The mapping of Eq. (3.5.34) is a parameterization of open subsets U i Ì C% of the
constraint manifold, equivalently the regular kinematic configuration space, by the variable v on
open subsets Vi Ì R ngc - nhc . This provides a mathematical representation of the kinematics of
mechanical systems on the Euclidean space R ngc - nhc , establishing the fact that the system has
ngc - nhc degrees of freedom in V 0 . The essential nonlinearities of the kinematic system are
imbedded in the function ψ(v ) . While the extent (size) of C% is not explicitly defined, an
important benefit of the fact that it is open is that once computation begins in C% , it can be
continued with confidence until the boundary of C% , if one exists, is encountered.
______________________________________________________________________________
Example 3.5.3 The Jacobian of Eq. (3.5.5) is not zero for all q Î C except q = [ ±p / 2 0 ] , so
T
the union of the four subsets of Eq. (3.5.6) without q1 = -p / 2, p / 2, or 3p / 2 is the regular
configuration space; i.e.,
C% = A
% ÈB
% ÈD
% È E% (3.5.42)
This defines four submanifolds C% 1 = A,% C% = B,
2
% C% = D,
3
% and C% = E% that are disjoint; i.e.,
4
% %
Ci Ç C j = j , the empty set, for i ¹ j and whose union is C = Èi C% i . This is an ad-hoc
%
observation at this point, but will be shown to be a general result in Section 3.6. In this example,
disjoint submanifolds of C% , called components, have singularities as their boundaries. This is not
always the case, as shown in Example 3.5.4.
______________________________________________________________________________
176
____________________________________________________________________________
Example 3.5.4 The slider-crank mechanism of Fig 3.5.6 has two generalized coordinates, as in
Example 3.5.1, and the single constraint equation
Φ(q ) = ( (cosq1 - q 2 ) 2 + (sin q1 ) 2 - 4 ) / 2 = 0 (3.5.43)
with Jacobian
Φq = [2q 2 sin q1 q 2 - cosq1 ] (3.5.44)
Since (q 2 - cosq1 )2 = 4 - (sin q1 )2 ¹ 0 , the Jacobian is never zero and there are no singularities.
Thus, C% = C .
Figure 3.5.6 Slider-Crank

Solving Eq. (3.5.43) for q 2 as a function of q1 yields two disjoint components of C% ,
{
C% plus = q : q 2 = cosq1 + (cosq1 )2 + 3 } (3.5.45)
C% minus = {q : q 2 = cosq1 - (cosq1 )2 + 3}
shown in Fig. 3.5.7 on the interval 0 £ q1 £ 2p , whose union is C% . Even though there are no
singularities in this example, there are multiple disjoint components of C% , associated with two
distinct assembled configurations.
Figure 3.5.7 C% plus (denoted plus) and C% minus (denoted minus)
______________________________________________________________________________
177
3.5.5 Computation of B(q) and h(v)
While the functions B(q) and h(v) of Eqs. (3.5.32) and (3.5.33) are shown to exist and be
differentiable functions of q and v, the derivation does not show how they may be evaluated.
Since they play a pivotal role in implementing the parameterization defined by Eqs. (3.5.34) and
(3.5.35), numerical methods for their evaluation are needed. This is pivotal in computational
kinematics and dynamics of mechanical systems. If the abstract mathematical approach to
manifold theory were employed, no insight into computation would be obtained.
At q 0 , B(q 0 ) = ( Φ q (q 0 ) U ) = ( U T U )
-1 -1
of Eq. (3.5.32) may be numerically evaluated.
For q in a neighborhood of q 0 , B(q ) must satisfy Eq. (3.5.32), written in the form
R = ( Φ q ( q )U ) B(q ) - I = 0 . With an approximation Bi = B(q0 ) of the solution and suppressing
the argument q, since it does not change in the iterative process for B( q ) , the matrix version of
Newton-Raphson iteration is defined by ( F q U ) DB i = - R i = -F q UBi + I . Since the matrix
( F U ) need not be inverted with great precision for use in the Newton-Raphson process and
q
B » ( F U ) , DB = -B F UB + B and B = B + DB . This yields the iterative algorithm

i -1 i i i i i +1 i i
q q
Bi +1 = 2Bi - B i F q UB i , i = 1, 2,L , until F q UBi +1 - I £ Btol (3.5.46)
where Btol is a specified error tolerance. This is an efficient computation, requiring only matrix
multiplication.
While the function h(v ) of Eq. (3.5.33) cannot be analytically determined, it can be
evaluated as accurately as desired using Newton-Raphson iteration to solve Eq. (3.5.30) for
u = h( v ) , with a given v; i.e., F u Du i = -Φq UDui = -BDui = -F (q0 + Vv - Uui ) . The solution is
Dui = BF(q0 + Vv - Uui ) and u i +1 = ui + Dui . This yields the iterative algorithm
u i+1 = u i + BF (q0 + Vv - Uu i ), i = 1, 2,... until F( q0 + Vv - Uu i +1 ) £ utol (3.5.47)
where utol is a specified error tolerance. Since the Newton-Raphson method does not require an
exact Jacobian, the matrix B is held constant throughout the process. This is an efficient
computation, requiring only matrix multiplication. With q extended to q j , an associated estimate
of B j » B j-1 is used with Eq. (3.5.46) to compute B j , continuing the process.
These results provide practical tools for local parameterization of constraint manifolds
for kinematics and dynamics of mechanical systems. A summary of the differential geometry of
constraint manifolds is presented in Section 3.6, as the basis for extension of these locally
defined tools to globally defined properties for rigorous kinematic and dynamic analysis of
mechanical systems.
178
The system configuration space is defined as the set of all configurations that satisfy
holonomic equations of constraint. It is shown that this space may contain singularities that can
be avoided by requiring the constraint Jacobian to have full rank on a subset, called the regular
configuration space.
A tangent space parameterization is defined for the regular configuration space, which is
shown to be a topological manifold. As illustrated in simple examples, the regular
configuration space may be comprised of disjoint components. Tangent space parameterization
of the holonomic constraint manifold provides both a constructive computational tool for
kinematic and dynamic analysis and an introduction to the underlying theory of manifolds.
Iterative numerical methods are presented for efficient and reliable construction of the tangent
space parameterization.
Key Formulas
C = {q : F (q ) = 0} Ì R ngc (3.5.2)
C% = {q Î C : rank Φq (q ) = nhc} Ì R ngc (3.5.6)
U = Φ Tq (q 0 ) Φ0q V = U T V = 0, V T V = I (3.5.23) (3.5.24)
q = q0 + Vv - Uh( v) Φ ( q0 + Vv - Uu ) = 0 u = h( v ) (3.5.34) (3.5.30) (3.5.33)
u i+1 = u i + BF (q0 + Vv - Uu i ), i = 1, 2,... until F( q0 + Vv - Uu i +1 ) £ utol (3.5.47)
B( q ) º ( Φ q ( q )U )
-1
(3.5.32)
Bi +1 = 2Bi - B i F q UB i , i = 1, 2,L , until F q UBi +1 - I £ Btol (3.5.46)
179
3.6 Differential Geometry of Constraint Manifolds
Results presented in Section 3.5 are local, in the sense that they are assured to hold only
in neighborhoods of configurations involved. For example, a differentiable manifold C% looks
like the Euclidean space R nhc on a chart (U,f ) , where f : U Ì R ngc ® R ngc -nhc , ngc > nhc , and
f is a homeomorphism. A metaphor is that a nearsighted person sees the neighborhood U as
Euclidean, even though C% is not Euclidean. This is akin to the early belief that the earth is flat.
A powerful impact of differential geometry is that it extends local results to global forms; akin to
an optometrist providing lenses to the nearsighted person that sees the earth as flat, showing its
nonlinear character by seeing well beyond a neighborhood of one’s feet. Differential geometry is
a tool, analogous to the corrective lenses, that enable observation in the large. The basics of
differential geometry on Euclidean space R ngc , in which mechanical systems function, are
summarized in this section and applied to constraint manifolds.
3.6.1 Euclidean Space
The Euclidean space,
{ T
R ngc = q = éëq1 L q ngc ûù , q i real } (3.6.1)
in which kinematics and dynamics of multibody systems take place, with addition of vectors and
T
multiplication of a vector by a real scalar defined by q1 + q2 = éëq11 + q12 L q1ngc + q 2ngc ùû and
T
aq = éëaq1 L aq ngc ùû , is a vector space of dimension ngc. There are ngc linearly independent
vectors ξ i Î R ngc that span R ngc ; i.e., for every q Î R ngc , there exist unique scalars
ngc
ai , i = 1,L, ngc , such that q = å α iξ i . With scalar product and norm defined by
i =1
ngc
q1 , q2 º q1Tq2 = å q1i q 2i (3.6.2)
1
q º q, q (3.6.3)
R ngc is a metric space (Mendelson, 1962). Defining a set N Ì R ngc to be open if for every vector
q0 Î N there exists a δ > 0 such that the open ball of radius δ centered at q0 ,
{ }
B(q0 ,δ) º q Î R ngc : q - q0 < δ , is a subset of N; i.e., B(q0 ,δ) Ì N . The Euclidean space with
topology t , denoted (R ngc , t) , is a topological space, where t is a collection of open sets in
R ngc with the following properties:
(1) R ngc and φ (the empty set) are open
(2) for any finite collection N i , i = 1,L, k , of open sets, the intersection Çingc i
=1 N is open
(3) for a possibly infinite collection N i , i Î I , of open sets, the union ÈiÎI N i is open
180
(4) there exists a possibly infinite collection of open sets N j , j Î J , such that
R ngc = È jÎJ N j
With its vector space and topological space structures, R ngc is a topological vector space.
The combination of these two properties form the foundation for a manifold structure that
establishes valuable theoretical results and practical methods for formulation and solution of
Lagrange multiplier-free ODE of multibody dynamics.
Topological properties of R ngc that form a foundation for multibody dynamics are
(Mendelson, 1962) as follows:
(1) R ngc is Hausdorff; i.e., for any vectors q1 ¹ q2 in R ngc , there exist open sets
N 1 and N 2 such that q i Î N i , i = 1,2 and N1 Ç N 2 = φ
(2) sets V Ì R ngc that are connected; i.e., there exist no nonempty open subsets
U1 and U 2 of R ngc such that V = U1 È U 2 and U1 Ç U 2 = φ , are path connected; i.e., for
any q i Î V, i = 1,2, there is a continuous curve q(t) = w(t) , 0 £ t £ 1, in V, defined by a
continuous vector function w(t) , such that q1 = w(0) and q2 = w(1)
(3) a subset W of R ngc with open sets defined as tW º {q Î N Ç W:N open in R ngc } Ì W
is a topological space. The topology tW is called the induced topology on W
(4) every topological subspace W of R ngc can be partitioned into maximal, path
connected, open subsets Wa Ì W, a Î I, called components, such that
(a) ÈaÎI Wa = W
(b) Wa Ç Wb = j if a ¹ b
3.6.2 Differential Geometry of the Regular Constraint Manifold

A consequence of the fact that the regular configuration space C% of Eq. (3.5.6), with the
tangent space parameterization of Eq. (3.5.34), is a differentiable manifold is that it is a
topological space in the topology induced by R ngc and, as summarized in Section 3.6.1, can be
partitioned into maximal, singularity free, path connected, components C% a , a Î I , such that
C% Ç C
a
% = j , for all a ¹ b , and C% = È C% . In particular, this shows that the number of
b aÎI a
degrees of freedom of the system is constant throughout each C% a . These facts are true in
Euclidean space, but not necessarily in abstract topological spaces. The submanifolds C% a are
maximal singularity free domains of kinematic functionality, abbreviated domains of
functionality. They are maximal, path connected, open subsets of C% , in each of which the
mechanism is locally parameterized by Eq. (3.5.34); i.e., there are no singular configurations in
C% a .
Extension of the local parameterization of Eq. (3.5.34) to a global setting on each C% a is a

powerful result that differential geometry brings to kinematics. While such results are
181
consequences of differential geometry, a deep study of the abstract theory of differential
geometry is not required for their use. Once established, implementation of these results requires
only use of linear algebra and multivariable calculus.
The components C% a are disjoint in C % , hence also disjoint in the ambient space R ngc . A
continuous transition of mechanism configuration from one such domain to another requires
crossing the boundaries of each in R ngc , if they have boundaries, in which case a continuous one-
dimensional trajectory q (t), 0 £ t £ 1 , in R ngc with q(0) Î C% a and q(1) Î C% b , a ¹ b , must pass
through ¶C % and ¶C% , hence encountering a singularity. This is illustrated schematically in Fig.
a b
3.6.1. The closures of some domains C% a ; e.g., C% 1 , may not intersect others; they may intersect at
a single point; e.g. C% and C% ; or they may intersect along common boundaries; e.g., C% and C% .
2 3 4 5
In any case, a continuous transition from one domain of functionality to another cannot occur
without encountering singular configurations. If closures of a pair of domains intersect, external
influence may be exerted to make a continuous transition across the common singular boundary,
so the mechanism does not need to be disassembled and reassembled to make the transition, but
it must encounter a singularity. In contrast, if closures of two domains do not intersect; e.g., in
Fig. 3.5.7, the mechanism must be disassembled and reassembled to make a transition between
them.
Figure 3.6.1 Closures of Domains of Functionality

If two charts defined in R ngc intersect in C% a , as shown schematically in Fig. 3.6.2, then
there is a differentiable mapping between intersecting subsets of the charts and the charts are
called compatible (Lee, 2010; Schlichtkrull, 2013), much as two planar maps of the surface of
the earth that cover the same locale are compatible. A family of compatible charts whose images
cover the entire manifold is an atlas. The atlas provides a structure upon which to characterize
trajectories on the constraint manifold, such as a solution curve for the equations of constrained
dynamics, as shown schematically in Fig. 3.6.2. The charts provide for changes in local
coordinates along a trajectory q( v(t)) in C% a , including differentiability of coordinates and
tangents. The theory of differentiable manifolds (Lee, 2010; Schlichtkrull, 2013) assures that a
solution trajectory can be continued in the open submanifold C% a until the constraint Jacobian
Φq (q ) is rank deficient. While the theory does not explicitly determine the extent of components
182
of the constraint manifold, it does assure that the parameterization by charts that has been
presented is valid on the largest possible subset of regular configuration space. Nothing more is
possible.
Figure 3.6.2 Continuation of Solution Trajectory over Charts

3.6.3 Examples
Elementary examples that illustrate the foregoing concepts are analyzed in the remainder
of this section. Further examples of singular configurations associated primarily with kinematic
drivers are presented in Section 3.7. Extensive analysis of singularities and singularity free
domains of functionality for manipulators is presented in Chapter 9.
______________________________________________________________________________
Example 3.6.1 The mechanism shown in Fig. 3.6.3, with generalized coordinates
q = [q1 q 2 q 3 ] Î R 3 and c º cosq 2 and s º sinq 2 , is subject to the holonomic constraint
T
Φ(q ) = ( (q1 + c - q 3 )2 + s2 - 4 ) / 2 = ( (q1 + c - q 3 ) 2 - c 2 - 3) / 2 = 0 (3.6.4)
with Jacobian
Φq (q ) = [(q1 + c - q 3 ) (q 3 - q1 )s -(q1 + c - q 3 )] (3.6.5)
Figure 3.6.3 Unequal Arm Length Slider-Crank Mechanism with ngc = 3
The solution of Eq. (3.6.4) is q 3 = q1 + c ± c 2 + 3 . Since (q1 + c - q 3 )2 = c 2 + 3 ¹ 0 ,

Φq (q) ¹ 0 for any configuration and C = C% has two 2-dimensional components in R 3 ,
{
% = q Î R 3 : q = q + c + c 2 + 3, q Î R 1 , q Î R1
C1 = C1 3 1 1 2 } (3.6.6)
%
C2 = C 2 = {q Î R : q
3
3 = q1 + c - c + 3, q Î R , q Î R }
2
1
1
2
1
183
Which, with the tangent space parameterization, are disjoint manifolds without boundary. This
result is similar to that of Example 3.5.4, but in one greater dimension.
______________________________________________________________________________
_____________________________________________________________________________
Example 3.6.2 Similar to Example 3.6.1, the mechanism shown in Fig. 3.6.4, with generalized
coordinates q = [q1 q 2 q 3 ] Î R 3 and c º cosq 2 and s º sinq 2 , is subject to the holonomic
T
constraint
Φ(q ) = ( (q1 + c - q 3 )2 + s2 - 1) / 2 = ( (q1 + c - q 3 )2 - c 2 ) / 2 = 0 (3.6.7)
with Jacobian
Φq (q ) = [(q1 + c - q 3 ) (q 3 - q1 )s -(q1 + c - q 3 )] (3.6.8)
The solution of Eq. (3.6.7) is q 3 = q1 + c ± c . Thus, the configuration space C has two 2-
dimensional subsets in R 3 ,
C1 = {q Î R 3 : q 3 = q1 , q1 Î R1 , q 2 Î R1}
(3.6.9)
C 2 = {q Î R 3 : q 3 = q1 + 2c, q1 Î R1 , q 2 Î R1 }
and C = C1 È C2 .
Figure 3.6.4 Equal Arm Length Slider-Crank Mechanism with ngc = 3

The Jacobian of Eq. (3.6.8) is rank deficient only if it is zero, which occurs if and only if
(q1 + c - q 3 ) = 0 and (q 3 - q1 )s = 0 . Using Eq. (3.6.7), this is c = 0 , so
q 2 = ±p / 2 and (q 3 - q1 ) = 0 ; i.e., the pair of one dimensional sets with q = [q1 ±p / 2 q1 ]
T
for arbitrary q1 Î R1 . Deleting these configurations from the subsets of configuration space of
Eq. (3.6.9) yields four components of C% , each of which is a topological manifold in its own
right,
C% 1 = {q Î R 3 : q 3 = q1 , q1 Î R1 , - p / 2 < q 2 < p / 2}
C% 2 = {q Î R 3 : q 3 = q1 , q1 Î R 1 , p / 2 < q 2 < 3p / 2}
(3.6.10)
C% 3 = {q Î R 3 : q 3 = q1 + 2c, q1 Î R1 , - p / 2 < q 2 < p / 2}
C% 4 = {q Î R 3 : q 3 = q1 + 2c, q1 Î R1 , p / 2 < q 2 < 3p / 2}
184
This example illustrates significant difficulties that arise if a mechanism has singular
configurations within the configuration space C. Nevertheless, differential geometry may be
systematically used to characterize kinematic properties of the mechanism.
_____________________________________________________________________________
A few observations are in order. While an enterprising person might draw pictures of the
manifolds for Examples 3.6.1 and 3.6.2 in R 3 , drawings of manifolds and in higher dimensional
spaces are out of the question. The theoretical existence of maximal path connected components
of manifolds in Euclidean space is realized in all examples treated in Sections 3.5 and 3.6, even
those with embedded singular configurations. As observed in Chapter one, mechanical system
kinematics is rife with nonlinearity. The only hope to deal effectively with this challenge is the
differential geometric construct that enables dealing with applications of moderate to large
dimension, without relying on detailed pictures, but with insight based on two- and three-
dimensional experience. Practical aspects of characterizing the manifolds as subsets of R ngc
remain unresolved.
Basic results of differential geometry show that open sets defined by local relations can be extended to
global sets that have properties such as being connected and path connected. It is especially important
that strong forms of these properties hold on Euclidean space, in which mechanical system kinematics
and dynamics occur.
The theory of differential topology shows that the regular configuration space is a manifold that is
comprised of maximal, singularity free, path connected components on which continuation of
numerical solution of the nonlinear equations of kinematics and dynamics can be carried out until
fundamental system singularities, if any exist, block progress.
185
3.7 Singular Configurations
Constraint equations are used for both kinematic and dynamic analysis of mechanical
systems in which motion is intended to proceed smoothly with time. For trial designs, however,
singular configurations may be encountered, beyond which motion cannot continue, more than
one possible motion can occur, or discontinuous motion can occur. To see that such behavior is
not restricted to pathological cases that are invented by a mathematician, several examples are
studied. Kinematic singularities that have nothing to do with the intended functionality of the
mechanism are studied in Sections 3.5.6 and 3.7.1. Singularities due to imposition of time
dependent drivers are analyzed in Section 3.7.2, and criteria for singular behavior are discussed
in Section 3.7.3.
3.7.1 Examples of Kinematic Singularities
_______________________________________________________________________
Example 3.7.1: Consider the two-body model of the slider-crank mechanism of Fig. 3.7.1. Body
one is a crank of unit length that rotates about the origin of the reference frame with angle q1
relative to the horizontal axis and body two slides along the horizontal axis, with coordinate q 2 .
The connecting rod is represented as a distance constraint of length l .
Figure 3.7.1 Slider-Crank Mechanism

The scalar holonomic constraint equation is
Φ ( q ) = ( (q 2 - c)2 + s 2 - l2 ) / 2 = 0 (3.7.1)
where c = cosq1 and s = sin q1 , the constraint Jacobian of which is
Φq ( q ) = [sq 2 q 2 - c] (3.7.2)
A kinematic singularity occurs in this case only if Φq (q ) = 0 ; i.e., sq 2 = 0 and q 2 = c .

The condition q 2 = c occurs only if the connecting rod is vertical, in which case s ¹ 0 . Thus,
q 2 = 0 and q1 = ± π/2 . This is possible only if l = 1 , so if l ¹ 1 there is no kinematic singularity.
If l = 1 , in the singular configuration q = [ p / 2 0] shown in Fig. 3.7.2, two modes of

T
motion are possible. First, q 2 can depart its zero value, with configurations shown at the left of
Fig. 3.7.3. Second, q 2 can be locked at its zero value and the crank and connecting rod can
remain colinear and rotate about the origin, as shown at the right of Fig. 3.7.3.
186
Figure 3.7.2 Slider-Crank at Singularity With l = 1
Figure 3.7.3 Admissible Motions of Slider-Crank at Singularity With l = 1

______________________________________________________________________________
_____________________________________________________________________________
Example 3.7.2: A two body model of a parallelogram mechanism is shown in Fig. 3.7.4. Bodies
one and two have unit length and are pivoted in ground, with angles of rotation q1 and q 2
relative to the horizontal axis. Their outboard ends are connected by a distance constraint of
length l . The distance between pivot points for bodies one and two is also l .
Figure 3.7.4 Parallelogram Mechanism

With the notation c i º cosq i and si º sin q i , i = 1,2 , the condition that the length of the
top bar is l is the holonomic constraint
Φ(q ) = ( (l + c 2 - c1 )2 + (s 2 - s1 )2 - l2 ) / 2 = 0 (3.7.3)
with Jacobian
187
Φq (q ) = [(l + c 2 - c1 )s1 ( l + c2 - c1 )( -s2 )]
= [ ls1 + (s1c 2 - c1s2 ) -ls2 - ((s1c 2 - c1s2 ))] (3.7.4)
= [ ls1 - sin(q 2 - q1 ) -ls2 + sin(q 2 - q1 )]
In any kinematically singular configuration, the constraint Jacobian is zero; i.e.,
ls1 = sin(q 2 - q1 ) and ls2 = sin(q 2 - q1 ) . Thus, s1 = s2 , so q1 = q 2 or q 2 = p - q1 . The singular
configuration with q1 = q 2 , s1 = s 2 , and q1 = q 2 = 0 is shown in Fig. 3.7.5. Admissible
configurations in a neighborhood of this singularity are shown in Fig. 3.7.6. If l = 1 a more
pathological singularity may occur, as shown in Fig. 3.7.7. In this singularity, q1 is locked at
q1 = 0 and q 2 is able to rotate freely, colinear with the coupler.
Figure 3.7.5 Singular Configuration with q1 = q 2 = 0
Figure 3.7.6 Configurations in Neighborhood of Singularity with q1 = q 2 = 0
Figure 3.7.7 Locked Singularity with q1 = 0 and l = 1

______________________________________________________________________________
3.7.2 Singularities Due to Time Dependent Drivers
If time dependent drivers are specified so that the Jacobian is square, singularities that are
not purely kinematic may arise. To see this, the slider-crank of Example 3.7.1 is revisited.
______________________________________________________________________________
Example 3.7.3: Imposing the driving constraint Φ D (q, t) = q1 - wt = 0 on the slider-crank of
Example 3.7.1, the combined kinematic and driving constraints are
é( (q - c)2 + s2 - l2 ) / 2 ù
F (q,t) = ê 2 ú=0 (3.7.5)
êë q1 - wt úû
whose Jacobian is
188
ésq q2 - cù
Fq (q, t) = ê 2
0 úû
(3.7.6)
ë 1
Thus, a singularity occurs if Fq (q, t) = -(q 2 - c) = 0 ; i.e., the connecting rod is vertical. If l > 1 ,
this cannot occur and there is no singularity. This is the case for practically designed slider-crank
mechanisms.
If l = 1 , the singularity shown in Fig. 3.7.2 will arise and motion corresponding to one of
the left configurations of Fig. 3.7.3 or the locked configuration with rotation q1 > p / 2 will
occur. If l < 1 , the connecting rod will be vertical in the singular configuration shown in Fig.
3.7.8. While this is not a kinematic singularity, in this configuration, Fq of Eq. (3.7.6) is singular
and the velocity equation Fq (q, t)q& = -Ft (q, t) will generally yield infinite velocity. Even more
likely, the acceleration equation Fq (q, t)q
&& = - g (q, t) will yield infinite acceleration.
Figure 3.7.8 Driver Singularity with l < 1

________________________________________________________________________
3.7.3 Criteria for Singular Behavior
Physical insight into singular behavior can be gained by studying the constraint velocity
equation for a system with square Jacobian,
Φq q& = -Φ t (3.7.7)
If Φq is singular at a time t*, Eq. (3.7.7) has a solution if and only if, by the theorem of the
alternative (Strang, 1980),
βT Φt = 0 (3.7.8)
for all solutions β of
ΦqTβ = 0 (3.7.9)
This result shows that Eq. (3.7.7) has a finite solution for velocity only if Φq and Φ t are
properly related.
Consider next a small virtual displacement dq that satisfies constraints to first order, with
time held fixed. Expanding the constraint equations to first order, dq satisfies
Φq dq = 0 (3.7.10)
Since Φq is singular at t*, its columns are linearly dependent and there exists a solution dq ¹ 0
of Eq. (3.7.10) that may be viewed as tangent to solution trajectories that emanate from q ( t*) .
189
Note that if a finite q& satisfies Eq. (3.7.7) and dq satisfies Eq. (3.7.10), then
Φq ( q& ± dq ) = Φq q& ± Φq dq = -Φ t (3.7.11)
Thus, two distinct velocities may occur after t*, which characterizes a bifurcation point.
Since the determinant Φq is zero at t*, except in cases for which the rank deficiency of
Φq is greater than 1, its sign must change as a bifurcation point is passed. This can serve as a
computational test for a bifurcation configuration. Only if no solution of Eq. (3.7.7) exists
beyond t*; i.e., only if q& approaches infinity as t approaches t*, is there no possibility of
continuing a solution. This characterizes a lock-up configuration. Thus, q& and/or q && approaching
infinity serves as criteria for lock-up.
A final note on the effect of a design variation on performance of a mechanism near a
singular point may help in evaluating the engineering feasibility of a design. Let b = [ b1 L b k ]
T
be a vector of design parameters, such as dimensions. Since these parameters arise in kinematic
constraint equations, the constraint equations of the mechanism are a function of design; i.e.,
Φ ( q,t;b ) = 0 (3.7.12)
There is a relationship between admissible virtual displacements dq and small variations db in

design. Equation (3.7.12) may be expanded to first order (linearized) as
Φq dq = -Φ b db (3.7.13)
With a singular configuration at time t*, Φq is singular. Thus, for some db , Eq. (3.7.13)
may have multiple solutions for dq , and for other db it may have no solutions. Physically, this
may be interpreted as indicating that, for some design variations, multiple changes in
configurations are possible, as in bifurcation, and for other design variations the mechanism
cannot be assembled.
________________________________________________________________________
Example 3.7.4: Consider the slider-crank model of Example 3.7.1, in the lock-up configuration
shown in Fig. 3.7.8. If l is a design parameter, a positive dl leads to two values of x 2 , as
shown in Fig. 3.7.9(a). A negative dl , however, leads to a design that cannot be assembled with
f1 = sin -1 l , as shown in Fig. 3.7.9(b).
190
Figure 3.7.9 Effect of Design Variation on Slider-Crank
________________________________________________________________________
Singular points that define lock-up or bifurcation are isolated; i.e., there are intervals of
time in which a unique solution of the equations of kinematics occurs on one or both sides of an
isolated singular point t*. Thus, except at t*, if the mechanism can be assembled, it is well
behaved in a neighborhood of the assembled configuration. Another form of singularity that is
commonly encountered is a redundant constraint. If a mechanism is modeled using standard
joints, it is possible that one or more of the kinematic constraint equations is identically satisfied
if the remaining constraints are satisfied, in which case the satisfied constraints are redundant and
consistent.
_______________________________________________________________________
Example 3.7.5: The five bar parallelogram mechanism of Fig. 3.7.10 is modeled as a single bar
with three distance constraints that represent bars of unit length that are pivoted in ground. With
q = [ x1 y1 f1 ] = [ x y f] , the kinematic constraint equations are
T T
é x 2 + y2 - 1 ù
ê 2 ú
Φ ( q ) = ê ( x + cos f - 1) + ( y + sin f - ) ú = 0
2
(3.7.14)
ê 2ú
ëê( x + 2cos f - 2 ) + ( y + 2sin f ) ûú
2
Figure 3.7.10 Five Bar Parallelogram Mechanism Model

The constraint Jacobian is
191
é 2x 2y 0 ù
ê ú
Φ q = ê 2 ( x + cos f - 1) 2 ( y + sin f - ) -2 ( x - 1) sin f + 2y cos f ú (3.7.15)
êë 2 ( x + 2 cos f - 2 ) 2 ( y + 2sin f ) -4 ( x - 2 ) sin f + 4 y cos fúû
Subtracting the first row from two times the second row and subtracting the result from the third
row leads to a matrix with a zero third row, for all values of q. Thus, the rank of the constraint
Jacobian is only two, and the third of the constraint equations of Eq. (3.7.14) is redundant. This
is clearly true from Fig. 3.7.10, since the third distance constraint is automatically satisfied by the
parallelogram that is defined with the first two distance constraints.
_________________________________________________________________________
Singular configurations of mechanical systems arise as a result of trial designs that lead to lock-
up or bifurcation, or poorly constructed models that have similar behavior. Examples studied
show that constraint Jacobian matrices are rank deficient at kinematically singular configurations.
In addition, imposing time dependent driving constraints can lead to singular behavior at
configurations that are not kinematic singularities. Singular behavior is shown to manifest itself in
velocities and/or accelerations diverging to infinity as singularities are approached. Finally,
design variations are shown to enable assessment of design deficiency in neighborhoods of
singular configurations.
192
3.8 Kinematic Position, Velocity, and Acceleration Analysis
A kinematic equation formulation is developed to include time dependent drivers, in
number equal to the number of degrees of freedom of the system. Derivatives are taken to obtain
velocity and acceleration equations. Iterative methods are presented for solution of nonlinear
kinematic equations for configurations on a time grid. Linear solution methods are presented for
velocities and accelerations on the same time grid. Numerical criteria are presented for
identification of singular behavior. Such kinematic analysis is seldom called for in systems with
nonholonomic constraints.
3.8.1 Generalized Coordinates and Constraint Equations
For a multibody system that is made up of nb bodies that are subjected to holonomic
constraints, a set of ngc generalized coordinates is defined to locate and orient each of the
bodies in the system. The generalized coordinate vector
T
q º éëq1T L qTnb ùû Î R ngc (3.8.1)
defines the positions and orientations of all bodies in the system. It is therefore defined as the
T
vector of system generalized coordinates. For planar body i, qi = éëriT fi ùû , and for spatial body
T
i, qi = éëriT p Ti ùû , where the Euler parameter vector must satisfy the condition p Tip i = 1 .
The kinematic model of a mechanism that is comprised of nb bodies is defined by a set of

constraint equations of the form
é F K (q ) ù
ê ú
F ( q,t ) º êF D ( q,t ) ú = 0 (3.8.2)
ê Fp (q ) ú
ë û
where Euler parameter normalization constraints for nsb spatial bodies are nsb equations of the
form
é p1T p1 - 1 ù
ê ú
Fp ( q ) = ê M ú =0 (3.8.3)
êp Tnsb p nsb - 1ú
ë û
and nhc holonomic kinematic constraints derived in Sections 3.2 and 3.3 are written in vector
function form as
F K (q) = 0 (3.8.4)
The total number of constraints in Eqs. (3.8.3) and (3.8.4) is nsb + nhc. In order that Eq. (3.8.2)
is comprised of ngc equations in ngc variables, nd = ngc - nsb - nhc driving constraints of
Sections 3.1 to 3.3 are defined in the form
F D ( q,t ) = 0 (3.8.5)
193
Let each of the constraint equations of Eq. (3.8.2) have m continuous derivatives with
respect to both q and t and define the ngc ´ ngc system constraint Jacobian as
é F qK ( q ) ù
ê ú
F q ( q,t ) = êF qD ( q,t )ú (3.8.6)
ê Fp q ú
ë q( ) û
At initial time t 0 , let q0 denote an assembled configuration of the mechanism; i.e.,
(
F q0 ,t 0 = 0) (3.8.7)
Under the above conditions, if the system constraint Jacobian is nonsingular at the initial time t 0
and associated assembled configuration q0 ; i.e.,
( )
F q q0 ,t 0 ¹ 0 (3.8.8)
the implicit function theorem of Section 2.2.5 guarantees that Eq. (3.8.2) has a unique, m-times
continuously differentiable solution q ( t ) ; i.e., F ( q ( t ) ,t ) = 0 , for all t in a neighborhood of t 0 .
Differentiating Eq. (3.8.2) with respect to time yields the system velocity equations,
F q ( q,t ) q& = -F t ( q,t ) º n ( q,t ) (3.8.9)
Since the kinematic and Euler parameter constraint functions of Eq. (3.8.2) do not depend
explicitly on time,
é 0 ù
n ( q,t ) = -F t ( q,t )ú
ê D
(3.8.10)
ê ú
ëê 0 ûú
Under the assumption that the system constraint Jacobian is square and nonsingular, Eq. (3.8.9)
has an ( m - 1) -times continuously differentiable solution for the generalized velocity q& ( t ) , for
all t in a neighborhood of t 0 .
Differentiating Eq. (3.8.9) with respect to time, using the chain rule of differentiation,
yields the system acceleration equations
&& = - ( F q ( q,t ) q& ) q& - 2F tq ( q,t ) q& - F tt ( q,t )
F q ( q,t ) q
q
(3.8.11)
= - g ( q,q& ,t ) - 2F tq ( q,t ) q& - F tt ( q,t )
Under the assumption that the system constraint Jacobian is nonsingular for all time, Eq. (3.8.11)
has an ( m - 2 ) -times continuously differentiable solution for the generalized acceleration q
&& ( t ) ,
for all t in a neighborhood of t 0 .
194
3.8.2 System Assembly
With a reasonable estimate of an assembled configuration q0 , the Newton-Raphson
method of Section 2.2.6 may be used for position analysis, hopefully converging to an accurate
solution for q0 . If that process fails, a method that is less dependent on a good estimate of q0 is
needed. One such approach is a numerical minimization method applied to the sum of squares of
constraint errors,
Y ( q ) º Φ ( q,t 0 ) Φ ( q,t 0 )
T
(3.8.12)
Modern numerical minimization methods are available, implemented in robust computer codes,
to begin with even a poor estimate of q0 and converge to an accurate solution.
3.8.3 Position Analysis
Equation (3.8.2) is a system of ngc nonlinear equations in a vector q containing ngc
variables that is to be determined in position analysis. Even if the constraint Jacobian is
nonsingular, there is little hope of finding an analytical solution q ( t ) , in spite of the fact that the
implicit function theorem guarantees existence of a unique solution with m continuous
derivatives. It is necessary, therefore, to resort to construction of a numerical approximate
solution at discrete points in time. At t = t i of a time grid t 0 , t1 , L , t i on which solutions of
kinematic equations are to be obtained, let an estimate of the solution of Eq. (3.8.2) be
q j » q i = q ( t i ) . Using the Newton-Raphson method of Section 2.2.7, the following linear
equations are solved:
( )
F q q j ,t i Dq j = -F q j ,t i ( ) (3.8.13)
q j+1 = q j + Dq j
The process continues until the following convergence criteria are met:
(
F q j+1 ,t i ) £e
(3.8.14)
Dq j+1 £ b
where e and b are solution error tolerances.

If the Jacobian and right side of the first of Eqs. (3.8.13) are evaluated accurately and the
Jacobian is nonsingular at all iterations, then Newton's method has attractive convergence
properties. In particular, for sufficiently small error q j - q ( t i ) in the initial estimate, the
method will be quadratically convergent. This is equivalent to the condition that there exists a
constant k such that for iteration number j sufficiently large,
q j+1 - q ( t i ) £ k q j - q ( t i )
2
(3.8.15)
If the initial estimate of the solution is substantially inaccurate, Newton's method may diverge.
195
3.8.4 Velocity and Acceleration Analysis
Velocities q& i » q& ( t i ) , i = 1, 2, L ; i.e., at the specified time steps, are determined by
solving the velocity equations
( ) ( )
F q qi ,t i q& i = -F t qi ,t i º n qi ,t i ( ) (3.8.16)
Note that no iterative computation is required, since the equations are linear in velocities.
&& ( t i ) , i = 1, 2, L , are
&&i » q
Much as in velocity analysis, in acceleration analysis, q
determined by solving the acceleration equations
( )
F q qi ,t i q ( ( ) )
&&i = - F q q i ,t i q& i
q
( ) ( ) ( )
q& i - 2F tq qi ,t i q& i - F tt qi ,t i º - g qi ,q& i , t i (3.8.17)
As in velocity analysis, no iterative computation is required, since the equations are linear in
accelerations.
3.8.5 Criteria for Singular Behavior
A time grid is normally defined with t i +1 = t i + h , i = 0, 1, 2,... , where h is the time step,
generally a small positive number. Given the numerical solution qi , q& i , and q
&&i at t i , an
approximate solution at t i +1 is estimated as
1
q ( t i +1 ) » qi + hq& i + h 2 q
&&i (3.8.18)
2
and Newton iteration for q i +1 is carried out using Eq. (3.8.13). In this way, approximate solutions
for q are found at each specified time step, as long as the constraint Jacobian is nonsingular.
Manifold theoretic results of section 3.6, illustrated in Fig. 3.6.2, show that this will be the case
until the boundary of the regular constraint manifold is encountered.
As noted in Section 3.7, a trial design may have singular configurations. Such
configurations may be identified during the foregoing kinematic analysis on the time grid
t 0 , t1 , t 2L . The most obvious indicator of an impending singular configuration is a precipitous
increase in &qi and &&qi , especially the latter. Another indicator is an increase in the number of
iterations required for convergence in Eq. (3.8.13). A third criteria is an increase in the condition
number, defined in Section 2.2.8, of the Jacobian F q (qi , t i ) that is factored in solution of the
first of Eqs. (3.8.13).
For holonomically constrained systems, time dependent drivers are defined to create the same
number of constraint equations as generalized coordinates. Kinematic analysis is then carried out,
using iterative methods to solve nonlinear constraint equations for an assembled configuration
and for kinematically admissible configurations on a specified time grid. Linear velocity and
acceleration equations are solved on the same time grid for dynamic response. Criteria for
singular behavior are monitored, including the condition number of the Jacobian, rapid changes in
velocity and acceleration, and divergence in position analysis.
196
Key Formulas
( ) ( )
F q qi ,t i Dqi = -F q i ,t i ; q i + = qi + Dqi (3.8.13)
Fq ( q ,t ) q& = -F ( q ,t ) º n ( q ,t )
i
i
i
t
i
i
i
i (3.8.16)
Fq ( q ,t ) q&& = - ( F ( q ,t ) q& ) q& - 2F ( q ,t ) q& - F ( q ,t ) º - g ( q ,q& , t )
i
i
i
q
i
i
i
q
i
tq
i
i
i
tt
i
i
i i
i (3.8.17)
197
3.9 Code 3.9 for Planar System Kinematic Analysis
The rudimentary general-purpose MATLAB computer Code 3.9 of Appendix 3.A
implements the kinematic analysis formulation of Section 3.8 for planar multibody systems.
Revolute, translational, and distance kinematic constraints and rotational drivers of Section 3.2
are included, with functions and derivatives presented in Section 3.2 that are used to implement
position, velocity, and acceleration analysis.
Following an explanation of Code 3.9 in this section, numerical examples are presented
in Section 3.10, including those treated with detailed derivations in Sections 3.2. The essence of
computational kinematics is enabling computer formulation and solution of the kinematic
equations, without the painful detail of ad-hoc derivation of equations of motion and explicit
coding of numerical solution algorithms. The computer implementation presented in this section
and applied in Section 3.10 is intended to introduce the reader to methods that are available in
commercial kinematic and dynamic simulation software and provide a tool for analysis of
mechanisms of the reader’s choice.
Components of Code 3.9 that interface with the user are presented in Section 3.9.1,
followed by an outline of the body of the code in Section 3.9.2, with which the user need not
interact.
3.9.1 User Components of Code
The initial segment of code involves definition of numerical solution parameters that
underlie the kinematic analysis formulation and associated numerical solution methods presented
in prior sections. Data included in the following discussion are for the two-body slider-crank
mechanism model of Fig. 3.2.6. Lines 3 to 7 of Fig. 3.9.1 define parameters that are used to
define time step and error control in the solution. Application data sets in the AppData function
of Fig. 3.9.2 are defined in lines 9 through 12 and selected for analysis in line 13.
1 %AA Planar Kinematic Analysis

2 %Error Control Parameters
3 qtol=10^-6; %Tolerance in solving for q
4 h=0.001; %Time step
5 tfinal=2*pi; %Final simulation time
6 maxiter=25; %Maximum number of iterations in Newton-Raphson iteration
7 maxCond=10^4; %Maximum condition number for Jacobian
8
9 %Application Data
10 %app=1, Slider-Crank
11 %app=2, Quick Return
12 %app=3, Windshield Wiper
13 app=3; %Select application for analysis
Figure 3.9.1 Analysis Parameters
The second component of user entered data is the AppData function that is shown for the
slider-crank in Fig. 3.9.2. The AppData function defines data that are entered for each
application, which are passed to the main code and functions executed in calls throughout the
code. Lines 10 through 14 define the dimension of variables that characterize the model. If these
data are entered incorrectly, the code will fail in ways that are difficult to understand from
MATLAB error messages.
198
The Planar Joint Data Table (PJDT) is defined in lines 16 through 24 of Fig. 3.9.2. Table
data for revolute, translational, and distance constraints and rotational drivers are defined in lines
17 through 20, with explanation expanded below. Line 17 defines elements of data in each row
of the table, with the function of each element defined in Lines 18 through 20, repeated as
follows for explanation:
16 %PJDT(12,nh): Planar Joint Data Table (First nh joints not time dependent)
17 %PJDT(:,k)=[T;i;j;sipr;sjpr;d;vipr;vjpr]
18 %k=joint No., T=joint type(1=Rev,2=Tran,3=Dist, 4=RotD, 5=DistD), i&j=bodies connected,
19 %sipr º s¢i &sjpr º s¢j =vectors to Pi&Pj in joint definition frame, d=dist.,
20 %vipr º v i¢ &vjpr º v¢j =vectors along translational axis
Time dependent drivers must occupy the bottom rows of the PJDT. Elements of data not
required for a joint are entered as zeroes.
To be more specific, a row of the PJDT that defines joint k as a revolute joint between
bodies i and j shown in Fig. 3.2.2, repeated here for clarity, is as follows:
PJDT(:,k)=[1;i;j; s¢i ; s¢j ;0;zer;zer]
Fig. 3.2.2 Revolute Joint

Elements of data not required for the revolute
joint are set to zero, where zer º [0 0] . The
T
user need enter only values k, i, j, s¢i , and s ¢j in

this template.
A row of the PJDT that defines joint k as a translational joint between bodies i and j
shown in Fig. 3.2.3, repeated here for clarity, is as follows:
PJDT(:,k)=[2;i;j; s¢i ; s¢j ;0; v ¢i ; v ¢j ]
Fig. 3.2.3 Translational Joint

The user need enter only values k, i, j,
s¢i , s¢j , v¢i , and v¢j in this template. While not
required, it is recommended that v ¢i and v¢j be
unit vectors.
A row of the PJDT that defines joint k as a distance constraint between bodies i and j
PJDT(:,k)=[3;i;j; s¢i ; s¢j ;d;zer;zer]
199
Fig. 3.2.1 Distance Constraint
The user need enter only values k, i, j, s¢i , s¢j ,
and d in this template, where zer º [0 0] .
T
A row of the PJDT that defines joint k as a relative angle driver between bodies i and j
that are connected by a revolute joint, which defines the constraint equation Eq. (3.2.20),
Φ rotD º f j - fi - b ( t ) = 0 , is as follows:
PJDT(:,k)=[4;i;j;zer;zer;0;zer;zer], where zer º [0 0] .

T
The user need enter only k, i, and j in this template. The time dependent term - b(t) and its first
and second time derivatives are entered in function P5.
A row of the PJDT that defines joint k as a relative distance driver between bodies i and j
that are connected by a revolute joint, which defines the constraint equation in Eq. (3.2.17),
( )
FdistD = dTijdi j - d ( t ) / 2 = 0 , is as follows:
2
PJDT(:,k)=[5;i;j; s¢i ; s¢j ;0;zer;zer], where zer º [0 0] .

T
The user need enter only k, i, j; s¢i , and s ¢j in this template. The time dependent term -d ( t ) / 2
2
and its first and second time derivatives are entered in function P5.
Finally, initial generalized coordinate estimates are defined in line 26.
q0e=[ r10 ; f10 ;… rnb
0
; f0nb ]
Data for the slider-crank of Eq. (3.2.33) in Section 3.2.3.3 are entered in lines 21 through
24 of Fig. 3.9.2.
8 if app==1 %Slider-Crank
9
10 nb=2; %Number of bodies
11 ngc=3*nb; %number of generalized coordinates
12 nh=3; %Number of holonomic constraints
13 nhc=5; %Number of constraint equations
14 nd=ngc-nhc; %Number of drivers
15
17 %PJDT(:,k)=[T;i;j;sipr;sjpr;d;vipr;vjpr]; k=joint No.,
18 %T=joint type(1=Rev,2=Tran,3=Dist, 4=RotD, 5=DistD), i&j=bodies connected,
19 %sipr&sjpr=vectors to Pi&Pj in joint definition frame, d=dist.,
20 %vipr&vjpr=vectors along translational axis
21 PJDT(:,1)=[1;1;0;zer;zer;0;zer;zer]; %Revolute-crank to ground
22 PJDT(:,2)=[2;2;0;zer;zer;0;ux;ux]; %Trans.-slider2 to ground
200
23 PJDT(:,3)=[3;1;2;ux;zer;1.01;zer;zer]; %Dist.-crank to slider2
24 PJDT(:,4)=[4;1;0;zer;zer;0;zer;zer]; %RelRotDrver-crank to ground
25
26 q0e=[0;0;0;2.2*ux;0]; %Initial generalized coordinate estimate
Figure 3.9.2 AppData Function
The third component of user entered code is the P5 function of Fig. 3.9.3 that defines
time dependent driver data, illustrated by the slider-crank. For each application, the position
driver vector, as a function of t n , is entered on line 9. Its first and second time derivatives are
entered on lines 11 and 13, respectively. These data are transferred by the code to the P5 vector
in lines 10, 12, and 14, for use throughout the code.
8 omega=1;
9 PDf=[omega*tn]; %Enter nd driver functions of tn
10 Pf=[zeros(nhc,1);PDf]; %Time dependent driver vector
11 PDfd=[omega]; %Enter nd first derivatives of driver functions of tn
12 Pfst=[zeros(nhc,1);PDfd]; %Time dependent driver velocity vector
13 PDfdd=[0]; %Enter nd second derivatives of driver functions of tn
14 Pfstt=[zeros(nhc,1);PDfdd]; %Time dependent driver acceleration vector
15 end
Figure 3.9.3 Time Dependent Driver Data in Function P5
3.9.2 Computational Components of Code
Computational flow in the main program, which requires little or no input from the user,
is presented in Fig. 3.9.4. Data for initiation of kinematic analysis are defined in lines 31 through
37. Iterative solution of position constraint equations and drivers, using the Newton-Raphson
method, is implemented in lines 41 through 66. Checks on condition number of the constraint
Jacobian and the number of iterations compared to the upper limit allowed are initiated in lines
52 and 60, respectively. If either test fails, the analysis is terminated with a warning. If the user
wishes to continue the analysis, limits maxCond and maxiter may be increased. Once the
configuration is known, velocity is calculated in lines 68 through 72. With configuration and
velocity known, acceleration is calculated in lines 74 through 78. Finally, output for each
application is defined, as for the slider-crank in lines 82 through 89. Code that define desired
output data are entered by the user for each application.
30 %Kinematic Analysis
31 n=1; %Index n
32 t(1)=0; %Initial time
33 while t(n)<tfinal %Loop through line 100
34 n=n+1;
35 t(n)=t(n-1)+h;
36 tn=t(n);
37 tnm=t(n-1);
38
39 [Pf,Pfst,Pfstt]=P5Eval(tn,par); %Evaluate time dependent terms
40
41 %Evaluate q
201
42 q=Q(:,n-1); %q-estimate
43 if n>1
44 q=q+h*Qd(:,n-1);
45 end
46 i=1; %Position iteration counter
47 err=qtol+1;
48 while err >qtol %Iteration for q, through line 65
49 Phi=PhiEval(q,PJDT,par);
50 Phiq=PhiqEval(q,PJDT,par);
51 PhiqCond=cond(Phiq);
52 if PhiqCond>maxCond %Check for ill conditioned constraint Jacobian
53 fprintf('Warning: Constraint Jacobian Ill Conditioned |n')
54 break
55 end
56 delq=-Phiq\(Phi+Pf); %Newton-Raphson increment
57 q=q+delq;
58 err=norm(Phi+Pf);
59 i=i+1;
60 if i>maxiter %Check for failure to converge in Newton-Raphson
61 fprintf('Warning: Newton-Raphson convergende failure')
62 break
63 end
64 end
65 iter(n)=i-1; %Report number of iteations
66 Q(:,n)=q; %Record q
67
68 %Evaluate qd
69 Phiq=PhiqEval(q,PJDT,par);
70 CondPhiq(n)=cond(Phiq); %Record condition number of Jacobian
71 qd=-Phiq\Pfst; %Solution of velocity equation
72 Qd(:,n)=qd; % Record qd
73
74 % Evaluate qdd
75 P2=P2Eval(q,qd,PJDT,par);
76 Gam=P2*qd+Pfstt;
77 qdd=-Phiq\Gam; %Solution of aceleration equation
78 Qdd(:,n)=qdd; %record qdd
79
80 %Calculate output data of interest (Enter for each application)
81
83 phi1(n)=q(3);
84 phi1d(n)=qd(3);
85 phi1dd(n)=qdd(3);
86 x2(n)=q(4);
87 x2d(n)=qd(4);
88 x2dd(n)=qdd(4);
89 end
Figure 3.9.4 Main Code Computational Flow
Computing functions that support the main code of Fig. 3.9.4 are identified in Fig. 3.9.5.
Computing functions include the Add function that adds nonzero submatrices to sparse matrices,
202
below and to the right of the address of the (1,1) term in the submatrix added to the underlying
matrix that was initialized to zeroes. ATran evaluates the orientation transformation matrix. The
user input function AppData is as presented in Fig. 3.9.2. Vector partition functions parPart and
qPart support partitioning of vectors involved into components that are used in kinematic
computations.
Computing Functions
Add
ATran
User Input Function
AppData
Vector Partition Functions
parPart
qPart
Constraint and Driver Partition Functions
DistPart
RevPart
TranPart
RelRotPart
Constraint and Constraint Derivative Evaluation Functions
PhiEval
PhiqEval
P2Eval
P5Eval
Figure 3.9.5 Computing Functions
The Constraint and Constraint Derivative Evaluation Functions and Constraint and Driver
Partition Functions shown in Fig. 3.9.5 are at the heart of kinematic and dynamic simulation.
Even though the user need not interact with these functions, it is important to understand how
they work.
The Constraint and Constraint Derivative Evaluation Functions listed in Fig. 3.9.5
evaluate kinematic constraint expressions and their derivatives that are defined in Section 3.2 and
are used in implementing the main code of Fig 3.9.4. As an illustration, the PhiEval function is
presented in Fig. 3.9.6. The vector Phi of constraint functions is initially set to zero in line 5. The
index k introduced in line 11 is varied over the range of holonomic and driving constraints in the
system, nh+nd. For a value of k, line 13 determines if the associate constraint is a revolute joint,
as defined in the AppData function and represented in the Planar Joint Data Table (PJDT). If so,
line 14 calls the associated Constraint and Data Table Partition Function of Fig 3.9.7 and enters i
and j as the bodies connected and joint data s¢i P and s¢jP . Lines 15 through 22 define body
generalized coordinates and orientation transformation matrices.
Care is taken to account for the fact that ground is designated by index j = 0 and its
generalized coordinates are constant, yielding no contribution in derivative evaluation functions,
but include geometric quantities that define the constraint of body i with ground. The revolute
joint constraint expression of Eq. (3.2.8) is evaluated in line 23. The Add function is then used in
line 24 to insert the revolute joint constraint value into the proper location in the vector Phi and
the address parameter m is indexed by the number of constraint expressions added in line 25.
203
The process is repeated in lines 28, 44, and 60 to determine if k represents a translational
constraint, distance constraint, or relative rotation driver. Associated time independent constraint
or driver values are entered in the constraint vector Phi. Index k is increased by one in line 73
and the process is repeated until all constraints and drivers are accounted for. If there are drivers,
the associated time dependent terms are entered in the P5 function.
1 function Phi=PhiEval(q,PJDT,par)
2
3 [nb,ngc,nh,nhc,nd,qtol,app]=parPart(par);
4
5 Phi=zeros(ngc,1); %Set system constraint vector to zero
6
7 I2=eye(2);
8 P=[0,-1;1,0];
9 k=1;
10 m=0;
11 while k<=nh+nd %Cycle through nh holonomic and nd driver constraints
12
13 if PJDT(1,k)==1 %Revolute constraint
14 [i,j,s1pr,s2pr]=RevPart(k,PJDT);
15 [r1,ph1]=qPart(q,i);
16 r2=[0;0];
17 ph2=0;
18 if j>=1 %If body j is not ground
19 [r2,ph2]=qPart(q,j);
20 end
21 A1=ATran(ph1);
22 A2=ATran(ph2);
23 PhiRev=r2+A2*s2pr-r1-A1*s1pr; %Evaluate revolute constraint
24 Phi=Add(Phi,PhiRev,m,0); %Add revolute contribution to Phi
25 m=m+2;
26 end
27
28 if PJDT(1,k)==2 %Translational constrint
29 [i,j,s1pr,s2pr,v1pr,v2pr]=TranPart(k,PJDT);
31 r2=[0;0];
32 ph2=0;
35 end
36 A1=ATran(ph1);
37 A2=ATran(ph2);
38 d12=r2+A2*s2pr-r1-A1*s1pr;
39 PhiTran=-[v1pr'*P*A1'*d12;v1pr'*P*A1'*A2*v2pr];
40 Phi=Add(Phi,PhiTran,m,0);
41 m=m+2;
42 end
43
44 if PJDT(1,k)==3 %Distance constraint
45 [i,j,s1pr,s2pr,d]=DistPart(k,PJDT);
204
47 r2=[0;0];
48 ph2=0;
51 end
52 A1=ATran(ph1);
53 A2=ATran(ph2);
54 d12=r2+A2*s2pr-r1-A1*s1pr;
55 PhiDist=(d12'*d12-d^2)/2;
56 Phi=Add(Phi,PhiDist,m,0);
57 m=m+1;
58 end
59
60 if PJDT(1,k)==4 %Relative rotation driver
61 [i,j]=RelRotPart(k,PJDT);
63 r2=[0;0];
64 ph2=0;
65 if j>=1
67 end
68 PhiRelRot=ph2-ph1;
69 Phi=Add(Phi,PhiRelRot,m,0);
70 m=m+1;
71 end
72
73 k=k+1;
74
75 end
Figure 3.9.6 Constraint Evaluation Function, PhiEval
The foregoing process is repeated in the PhiqEval, P2Eval, and P5Eval functions. The
sequence of calculation is identical for each, with the function Phi replaced by Phiq, P2, and PDf.
Finally, the Constraint and Driver Partition Functions provide access to elements of the
Planar Joint Data Table, PJDT, that is entered in the AppData function to control computation, as
illustrated in Fig. 3.9.6. To illustrate use of these functions, the RevPart function that appears in
line 14 of Fig. 3.9.6 is presented in Fig. 3.9.7. Once line 13 of Fig 3.9.6 determines that the value
k designates a revolute joint, the function of Fig. 3.9.7 is called in line 14 to provide data needed
on bodies connected and connection data that are defined in the Planar Joint Data Table of the
AppData function. Comparable functions DistPart, TranPart, and RelRotPart define data for the
associated constraints and drivers.
1 function [i,j,s1pr,s2pr]=RevPart(k,PJDT)
2
3 i=PJDT(2,k);
4 j=PJDT(3,k);
5 s1pr=[PJDT(4,k);PJDT(5,k)];
6 s2pr=[PJDT(6,k);PJDT(7,k)];
7 end
Figure 3.9.7 Constraint Partition Function, RevPart
205
3.9.3 Code Output
In addition to output defined for each application in the AppData function, the code
reports the following arrays that shed light on the application and performance of the code:
CondPhiq: the condition number of the Jacobian F q at each time step
iter; the number of Newton-Raphson iterations required at each time step
Q; the array of values of q at each time step
Qd; the array of values of q& at each time step
Qdd; the array of values of q&& at each time step
The rudimentary MATLAB Code 3.9 of Appendix 5.A is a tool for numerical simulation and
experimentation with the kinematics of planar multibody systems. It is typical of code that
enables the engineer to create models of mechanical systems, carry out simulations, and
investigate the effects of model and computational variations, without investing extensive time
and frustration in ad-hoc derivation and coding of specific applications. The structure of the
kinematic analysis code is identical to more extensive codes for system dynamics. In fact, it
provides the kinematic underpinning of those codes.
206
3.10 Planar System Kinematic Analysis Using Code 3.9
The equations derived in Section 3.2 and implemented in Code 3.9 that is presented in
Section 3.9 form the foundation for computer aided kinematic analysis of planar systems. A
slider-crank mechanism that is subject to near singular conditions, a moderate dimensional quick
return mechanism, and a windshield wiper mechanism are studied in the following subsections.
3.10.1 Planar Slider-Crank
The two-body slider-crank model of Section 3.2.3.3 is analyzed, using MATLAB Code
3.9 of Appendix 3.A and data presented in Eq. (3.2.33) and the AppData function of Fig. 3.10.1.
The angle driver that defines constant angular velocity of the crank is f& 1 - omega ´ t = 0 , with
time dependent driver data entered in function P5 shown in Fig. 3.9.3. To study the effect of
varying crank angular velocity, crank radius and connecting rod length are selected as R = 1 m
and d = 1.2 m, respectively. Plots of position, velocity, and acceleration (x2, x2d, and x2dd) of
the slider for crank angular velocities omega = 1, 2, and 4 rad/sec are presented in Fig. 3.10.2,
over the time interval [0 2p] ; i.e., 1, 2, and 4 revolutions of the crank. As expected, velocity
varies linearly with input angular velocity and acceleration varies with the square of velocity.
9
13 nhc=5; %Number of constraint equations
14 nd=ngc-nhc; %Number of drivers
15
18 %T=joint type(1=Rev,2=Tran,3=Dist, 4=RotD), i&j=bodies connected,
19 %sipr&sjpr=vectors to Pi&Pj in joint definition, d=dist.,
20 %vipr&vjpr=vectors along translstional axis
24 PJDT(:,4)=[4;1;0;zer;zer;0;zer;zer]; %RelRotDrver-crank to ground
25
26 q0e=[0;0;0;2.2*ux;0]; %Initial generalized coordinate estimate
Figure 3.10.1 AppData Function for Slider-Crank
207
Figure 3.10.2 Position, Velocity, and Acceleration of Slider, R = 1 m and d = 1.2 m
To study the influence of variable connecting rod length, the crank of radius R = 1 is
driven at omeg = 1 rad/sec and connecting rod lengths are selected as d = 2, 1.2, and 1.01 m.
Plots of position, velocity, and acceleration of the slider are presented in Fig. 3.10.3. Since a
connecting rod length of 1 m is a singular configuration, it is not surprising that accelerations,
hence constraint reaction forces, become extreme with a connecting rod length of 1.01 m.
208
Figure 3.10.3 Near Singular Behavior of Slider-Crank, omega =1 rad/sec
3.10.2 Quick Return Mechanism
The quick return mechanism shown in Fig. 3.10.4 is driven by counterclockwise rotation
of the flywheel of body 2 to cause oscillation of body 1, via the body 3 key that translates in the
slot shown in body 1. The connecting rod between point P1 on body 1 and the origin of the
cutting tool reference frame on body 4 causes body 4 to move slowly to the left in a cutting
stroke and to return more quickly to the right for the next cutting stroke. The radius of the
flywheel is 2 m, the distance between points O and P1 on body 1 is 4 m, and the length of the
connecting rod that is modeled as a distance constraint is 2.5298 m. These data and the initial
generalized coordinate estimate for the mechanism are given in the AppData function of Fig.
209
3.10.5. A rotation driver is applied to the flywheel as f2 - omega ´ t n = 0 , with the time
dependent driving function and its derivatives entered in the P5 function.
Figure 3.10.4 Quick Return Mechanism
30 if app==2 %Quick Return

31
34 nh=6; %Number of time independent holonomic constraints
35 nhc=11; %Number of time independent holonomic constraint equations
36 nd=ngc-nhc; %Number of time dependent driving constraint equations
37
38
39 ux=[1;0];
40 uy=[0;1];
41 zer=zeros(2,1);
42
43 %PJDT(12,nh): Planar Joint Data Table (First nh not time dependent)
45 %T=joint type(1=Rev,2=Tran,3=Dist, 4=RotD), i&j=bodies conn.,
46 %si&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=vectors along trans axis
47 PJDT(:,1)=[1;1;0;-2*ux;zer;0;zer;zer]; %Revolute-bar to ground
48 PJDT(:,2)=[1;2;0;zer;2*uy;0;zer;zer]; %Revolute-crank to ground
49 PJDT(:,3)=[1;2;3;1.5*ux;zer;0;zer;zer]; %Revolute-crank to key
50 PJDT(:,4)=[2;1;3;zer;zer;0;ux;ux]; %Trans.-bar to key
51 PJDT(:,5)=[2;4;0;zer;4*uy;0;ux;ux]; %Trans.-cutter to ground
52 PJDT(:,6)=[3;1;4;2*ux;zer;2.5298;zer;zer]; %Dist.-bar to cutter
53 PJDT(:,7)=[4;2;0;zer;zer;0;zer;zer]; %RotD-crank to ground
54
55 %Initial generalized coordinate estimate
56 q10=[1.2;1.6;0.9273];
57 q20=[0;2;0];
58 q30=[1.5;2;0.9273];
59 q40=[0;4;0];
60 q0e=[q10;q20;q30;q40];
Figure 3.10.5 AppData Function for Quick Return Mechanism
210
Plots of position and velocity of the slider, x4 and x4d, for angular velocity omega = 1, 2,
and 4 rad/sec of the crank are shown in Fig. 3.10.6. The initial slow motion of the cutter and the
quick return are clearly shown. As the crank angular velocity increases, cutter velocity increases
linearly.
Figure 3.10.6 Cutter Position and Velocity, x4 and x4d
3.10.3 Windshield Wiper

The windshield wiper model of Section 3.2.3.4 is analyzed using MATLAB Code 3.9.
Data presented in Eq. (3.2.37) are implemented with a relative rotation driver f& 1 - omega ´ t = 0
211
and initial generalized coordinate estimates in the AppData function of Fig. 3.10.7. The time
dependent driving function and its derivatives are entered in the P5 function.
64 if app==3 %Windshield Wiper

65
68 nh=5; %Number of time independent holonomic constraints
69 nhc=8; %Number of time independent holonomic constraint equations
70 nd=ngc-nhc; %Number of time dependent driving constraint equations
71
72 ux=[1;0];
73 uy=[0;1];
74 zer=zeros(2,1);
75
76 %PJDT(12,nh): Planar Joint Data Table (First nh not time dependent)
78 %T=joint type(1=Rev,2=Tran,3=Dist, 4=RotD), i&j=bodies conn.,
79 %sipr&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=vectors along trans axis
80 PJDT(:,1)=[1;1;0;zer;zer;0;zer;zer]; %Rev-bod 1 to ground
81 PJDT(:,2)=[1;2;0;zer;-70*ux;0;zer;zer]; %Rev-bod 2 to ground
82 PJDT(:,3)=[1;3;0;zer;30*ux;0;zer;zer]; %Rev-bod 3 to ground
83 PJDT(:,4)=[3;1;2;-5*uy;-8*uy;70;zer;zer]; %Dist.-bod 1 to bod 2
84 PJDT(:,5)=[3;2;3;5*uy;5*uy;100;zer;zer]; %Dist.-bod 2 to bod 3
85 PJDT(:,6)=[4;1;0;zer;zer;0;zer;zer]; %RotD-bod 1 to ground
86
88 q10=[0;0;0];
89 q20=[-70;0;0];
90 q30=[30;0;0];
91 q0e=[q10;q20;q30];
Figure 3.10.7 AppData Function for Windshield Wiper
Plots of left wiper angle, angular velocity, and angular acceleration (phi2, phi2d, and
phi2dd) on the time interval [0 2p] are presented in Fig. 3.10.8 for input angular velocities
omega = 1 and 2 rad/sec. These inputs correspond roughly to a low- and high-speed setting
during light and heavy rain, respectively. The nonlinear character of the kinematic equations, in
particular the quadratic dependence of the right side of the kinematic acceleration equations on
velocity, is reflected in significantly higher accelerations in the case omega = 2 rad/sec. Plots of
right wiper angle, angular velocity, and angular acceleration are essentially identical, as expected
due to symmetry of the coupling between left and right wipers. As with the slider-crank
example, velocity varies linearly with input angular velocity and acceleration varies with the
square of velocity.
212
Figure 3.10.8 Left Wiper Angle, Angular Velocity, and Angular Acceleration
For system models with time dependent drivers that create the same number of constraint
equations as generalized coordinates, kinematic analysis is carried out using iterative methods to
solve nonlinear constraint equations for kinematically admissible configurations on a time grid
that is specified for analysis. Linear velocity and acceleration equations are solved on the same
time grid for dynamic response. Criteria for singular behavior are monitored, including the
condition number of the Constraint Jacobian, rapid changes in acceleration, and loss of existence
in position analysis.
Numerical analyses using Code 3.9 of Appendix 3.A are carried out with planar examples.
Results confirm the criteria for onset of singular behavior and quadratic dependence of system
acceleration on velocities.
213
3.11 Code 3.11 for Spatial System Kinematic Analysis
The rudimentary general-purpose MATLAB Code 3.11 of Appendix 3.A implements the
kinematic analysis formulation of Sections 3.3 and 3.8 for spatial multibody systems. Building
block distance, spherical, dot1, and dot2 constraints are implemented and serve as the foundation
for cylindrical, revolute, translational, distance, universal, strut, and revolute-spherical kinematic
constraints of Section 3.3. Equations of Section 3.3 are used to implement position, velocity, and
acceleration analysis. Following an explanation of Code 3.11 in this section, numerical examples
are presented in Section 3.12.
followed by an outline of the body of the code, with which the user need not interact, in Section
3.11.2. The structure of Code 3.11 is identical to that of Code 3.9 presented in Section 3.9.
References are made to that code and Section 3.9, where appropriate, to avoid unnecessary
repetition.
The initial segment of code involves definition of numerical solution parameters that
underlie the kinematic analysis formulation and associated numerical solution. Since it is
identical to the code of Fig 3.9.1, the reader is referred to Section 3.9.1 for discussion of its use.
Application data are indexed in lines 9 to 14 of Fig. 3.11.1 to specific applications that
are defined in the AppData function presented in the following. The declaration in line 15
defines which application is implemented in the analysis, the spatial slider-crank in this section.
Data that are returned from the AppData function in line 17 and parameters in the par vector of
line 18 are used throughout the code to pass data for each application. Data and storage arrays
that are used throughout the code are defined in lines 20 through 29 and the initial generalized
coordinate estimate q0e defined in the AppData function is entered in Q in line 31.
1 %AA Spatial Kinematic Analysis
2 % User Input Section:
3 qtol=10^-6; %Tolerance in solving for u
4 maxiter=25; %Maximum number of iterations in Newton-Raphson iteration
5 maxCond=10^4; %Maximum condition number for Jacobian
6 h=0.01; %Time step
7 tfinal=4*pi; %Final simulation time
8
9 %Applications Defined in AppData Function
10 %app=1, 2-Bar with 2 rotational drivers
11 %app=2, 4-Bar with internal rotational driver
12 %app=3, 2-body Slider in x-z plane, distDr Bod1-grnd
13 %app=4, Slider-Crank in y-z Plane, Bod 1 RotDr about x axis
14 %app=5, Spatial Slider-Crank, RotDr Bod1-grnd
15 app=5; % Select which application to simulate
16
17 [nb,ngc,nh,nhc,nc,nd,SJDT,q0e]=AppData(app); %Data Defined in AppData(app) Function for app selected
18 par=[nb;ngc;nh;nhc;nd;qtol;app]; %Parameter vector for analysis control
19
20 global ux uy uz z3 %Make vectors available to all functions
21 ux=[1;0;0];
22 uy=[0;1;0];
214
23 uz=[0;0;1];
24 z3=zeros(3,1);
25
26 % Data Storage Arrays
27 Q=zeros(ngc,10);
28 Qd=zeros(ngc,10);
29 Qdd=zeros(ngc,10);
30
31 Q(:,1)=q0e;
Figure 3.11.1 Analysis Parameters and Data Storage
The third component of user entered code is the AppData Function of Fig. 3.11.2 for the
spatial slider-crank mechanism of Section 3.3.7.2 in Fig. 3.3.13. Data entered in the AppData
function are passed to the main code for functions executed in calls throughout the code. Lines
129 through 134 define dimensions of variables used in the model. If these data are entered
incorrectly, the code will fail in ways that are difficult to understand from MATLAB error
messages.
The Spatial Joint Data Table (SJDT) is defined in lines 136 through 144 of Fig. 3.11.2.
Table data for distance, spherical, cylindrical, revolute, translational, universal, strut, and
revolute-spherical constraints and distance and rotational drivers are defined in lines 137 through
140, with explanation expanded, as follows:
136 %SJDT(22,nh): Spatial Joint Data Table

137 %SJDT(:,k)=[T;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr];
138 %k=joint No.; T=joint type(1=Dist,2=Sph,3=Cyl, 4=Rev, 5=Tran,
139 %6=Univ, 7=Strut, 8=Rev-Sph, 9=DistDr, 10=RotDr); i&j=bodies connected, i>0;
140 %sipr&sjpr=vectors to Pi&Pj; d=dist.; vxipr, vzipr, vxjpr, vzjpr=vectors in joints.
Line 137 defines elements of data in each row of the table, with the function of each element
defined in Lines 138 through 140. Time dependent drivers must occupy the bottom rows of the
SJDT. Elements of data not required for a joint are entered as zeroes.
To be more specific, a row of the SJDT that defines joint k as a distance constraint
between bodies i and j shown in Fig. 3.3.2, repeated here for clarity, is as follows:
SJDT(:,k)=[1;i;j; s¢i ; s¢j ;d;z3;z3;z3;z3]
Fig. 3.3.2 Distance Constraint

The user need enter only k, i, j, s¢i , s¢j , and d in
this template. Elements of data not required
for the distance constraint are set to zero,
where z3 º [0 0 0] .
T
215
A row of the SJDT that defines joint k as a spherical constraint between bodies i and j
SJDT(:,k)=[2;i;j; s¢i ; s¢j ;0;z3;z3;z3;z3]
Fig 3.3.3 Spherical Joint

The user need only enter k, i, j, s¢i and s¢j for
this joint, where z3 º [0 0 0] .
T
Rows of the SJDT that define joint k as a cylindrical constraint, revolute constraint, or
translational constraint, respectively between bodies i and j shown in Fig. 3.3.4, repeated here
for clarity, are as follows:
SJDT(:,k)=[3;i;j; s¢i ; s¢j ;0; v ¢xi ; v¢zi ; v¢xj ; v ¢zj ]
Fig. 3.3.4 Cylindrical, Revolute, and

Translational Joints
Vectors s¢i and s¢j that locate points Pi and Pj
on bodies i and j are as shown in Fig. 3.3.2. In
addition to these vectors, the user must enter
k, i, and j and unit vectors
v ¢xi , v ¢zi , v¢xj , and v ¢zj . Orthogonality
conditions that define the respective
constraints are defined in Sections 3.3.3.1
through 3.3.3.3.
216
A row of the SJDT that defines joint k as a universal joint between bodies i and j shown
in Fig. 3.3.5, repeated here for clarity, is as follows:
SJDT(:,k)=[6;i;j; s¢i ; s¢j ;0;z3; v ¢zi ;z3 ; v¢zj ]
Fig. 3.3.5 Universal Joint

Vectors s¢i and s¢j that locate point P on bodies
i and j are as shown in Fig. 3.3.2. In addition
to these vectors, the user must enter k, i, j, and
body fixed unit vectors v ¢zi and v¢zj on the
cross, where z3 º [0 0 0] .
T
A row of the SJDT that defines joint k as a strut joint between bodies i and j shown in
Fig. 3.3.8, repeated here for clarity, is as follows:
SJDT(:,k)=[7;i;j; s¢i ; s¢j ;0;z3;z3 ; v¢xj ; v¢zj ]
Fig. 3.3.8 Strut Composite Joint

k, i, j, and the body fixed unit vector v¢zj on
body j, where z3 º [0 0 0] .
T
A row of the SJDT that defines joint k as a revolute-spherical joint between bodies i and j
SJDT(:,k)=[8;i;j; s¢i ; s¢j ;d;z3;z3;z3 ; v¢zj ]
Fig. 3.3.9 Revolute-Spherical Joint

k, i, j, d, and the body fixed unit vector v¢zj on
body j, where z3 º [0 0 0] .
T
217
A row of the SJDT that defines joint k as a relative distance driver between bodies i and
j, as shown in Fig. 3.3.2, which imposes the constraint equation in Eq. (3.3.38),
( )
FdistD = dTijdi j - d ( t ) / 2 = 0 where di j is as defined for the distance constraint, is as follows:
2
SJDT(:,k)=[9;i;j; s¢i ; s¢j ;0;z3;z3], where z3 º [0 0 0] .

T
The user need enter only k, i, j, s¢i , and s ¢j in this template. The time dependent term -d ( t ) / 2
2
and its first and second time derivatives are entered in function P5.
A row of the SJDT that defines joint k as a relative rotation driver between bodies i and j
shown in Fig. 3.3.10 that are connected by a revolute or cylindrical joint, repeated here for
clarity, is as follows
Fig. 3.3.10 Relative Rotation Drivers

Vectors s¢i , s¢j , v ¢xi , v ¢zi , v ¢xj , and v¢zj are as
defined in the host revolute or cylindrical
joint shown in Fig. 3.3.10. The time
dependent relative rotation qi j (t) in Eq.
(3.3.39) and its first and second time
derivatives are specified in function P5.
Initial generalized coordinate estimates defined in lines 147 to 151 are used to initiate
iterative solution for system configuration in the main code.
127 if app==5 %Spatial Slider-Crank, Bod 1 RotDr

128
132 nhc=11; %Number of holonomic constraint equations
133 nc=nhc+nb; %Number of constraint equations
134 nd=ngc-nc;
135
139 %6=Univ, 7=Strut, 8=Rev-Sph, 9=DistDr, 10=RotDr); i&j=bodies connected, i>0;
140 %sipr&sjpr=vectors to Pi&Pj; d=dist.; vxipr, vzipr, vxjpr, vzjpr=vectors in cyl, rev, and tran.
141 SJDT(:,1)=[4;1;0;z3;0.1*uy+0.12*uz;0;uz;ux;uz;ux]; %Rev-Body1 to Ground
142 SJDT(:,2)=[5;2;0;z3;z3;0;uz;ux;uz;ux]; %Tran-Body2 to Ground
143 SJDT(:,3)=[1;1;2;0.08*uz;z3;0.24;z3;z3;z3;z3]; %Dist-Body 1 to 2
144 SJDT(:,4)=[10;1;0;z3;0.1*uy+0.12*uz;0;uz;ux;uz;ux]; %RotDr-Body 1 to Ground
145
218
147 r10=[0;0.1;0.12];
148 p10=[1;0;0;0];
149 r20=[0.25;0;0];
150 p20=[1;0;0;0];
151 q0e=[r10;p10;r20;p20];
152
153 end
Figure 3.11.2 AppData Function, Spatial Slider-Crank
The final component of user entered code is the P5 Function of Fig. 3.11.3 for the spatial
slider-crank mechanism of Section 3.3.7.2 in Fig. 3.3.13. Using the RotDrPart(k,SJDT) function in
line 223, data on bodies connected are obtained for evaluation of terms in the driving constraints
needed, with time dependent θ i j (t) defined in P5 for kinematic analysis. Data from the P5
function are used in the main code to define time dependence of constraints in position, velocity,
and acceleration analysis. As in the planar kinematics Code 3.9, driving constraints follow
kinematic constraints in the SJDT. In the spatial code, however, Euler parameter constraints
follow driving constraints. This is illustrated in lines 263 to 265 of function P5 in Fig. 3.11.3,
which computes right sides of position, velocity, and acceleration constraint equations; P, Pst,
and Pstt.
221 if app ==5 %Spatial Slider-Crank, RotDr Bod1-grnd

222 k=4;
223 [i,j,vx1pr,vy1pr,vx2pr]=RotDrPart(k,SJDT);
224
225 omega=4;
226 theta=omega*tn;
227 thetad=omega;
228 thetadd=0;
229
230 [r1,p1]=qPart(q,i);
231 A1=ATran(p1);
232
233 if j==0
234 c=vx1pr'*A1'*vx2pr;
235 s=vy1pr'*A1'*vx2pr;
236 if abs(c)>=abs(s)
237 PD=-sin(theta);
238 PDd=-thetad*cos(theta);
239 PDdd=-thetadd*cos(theta)+(thetad^2)*sin(theta);
240 else
241 PD=-cos(theta);
242 PDd=thetad*sin(theta);
243 PDdd=thetadd*sin(theta)+(thetad^2)*cos(theta);
244 end
245 end
246
247 if j>=1
248 [r2,p2]=qPart(q,j);
249 A2=ATran(p2);
250 c=vx1pr'*A1'*A2*vx2pr;
219
251 s=vy1pr'*A1'*A2*vx2pr;
253 PD=-sin(theta);
256 else
257 PD=-cos(theta);
260 end
261 end
262
263 P=[zeros(nhc,1);PD;zeros(nb,1)];
264 Pst=[zeros(nhc,1);PDd;zeros(nb,1)];
265 Pstt=[zeros(nhc,1);PDdd;zeros(nb,1)];
266 end
Figure 3.11.3 P5 Function, Spatial Slider-Crank
Computational flow in the main program, which requires no input from the user, from
line 33 through 86 is outlined in Fig. 3.11.4. Data to be reported for each application are entered
by the user following line 88, with output data for the spatial slider-crank illustrated in lines 180
through 193.
33 %Kinematic Analysis
34 n=1;
35 t(1)=0;
36 while t(n)<tfinal
37 n=n+1;
38 t(n)=t(n-1)+h;
39 tn=t(n);
40 tnm=t(n-1);
41
42 %Evaluate q
43
44 q=Q(:,n-1); %q-estimate
45 if n-1>1
46 q=q+h*Qd(:,n-1);
47 end
48
49 i=1; %Position iteration counter
50 err=qtol+1;
51 while err >qtol
52 Phi=PhiEval(tn,q,SJDT,par);
53 Phiq=PhiqEval(q,SJDT,par);
54 if i==1
55 CondPhiq(n)=cond(Phiq);
56 if CondPhiq(n)>maxCond %Check for ill conditioned constraint Jacobian
57 fprintf('Warning: Constraint Jacobian Ill Conditioned |n')
58 break
59 end
220
60 end
61 delq=-Phiq\Phi;
62 q=q+delq;
63 err=norm(Phi);
64 errrpt(i)=err;
65 i=i+1;
66 if i>maxiter %Check for failure to converge in Newton-Raphson
67 fprintf('Warning: Newton-Raphson convergende failure')
68 break
69 end
70 end
71
72 iter(n)=i-1; %Report number of iteations
73 Q(:,n)=q; %Record q
74
75 %Evaluate qd
76 Phiq=PhiqEval(q,SJDT,par);
77 [P,Pst,Pstt]=P5Eval(tn,q,SJDT,par);
78 qd=-Phiq\Pst;
79 Qd(:,n)=qd; % Record qd
80
81 % Evaluate qdd
82 %P2=P2Eval(q,qd,SJDT,par);
83 %Gam=P2*qd+Pstt;
84 Gam=GamEval(tn,q,qd,SJDT,par);
85 qdd=-Phiq\Gam;
86 Qdd(:,n)=qdd; %record qdd
87
88 %Calculate output data of interest (Enter for each application)
180 if app==5 %Spatial Slider-Crank, RotDr Bod1-grnd

181 thetax(1)=0;
182 [r1,p1]=qPart(q,1);
183 [r1d,p1d]=qPart(qd,1);
184 [r1dd,p1dd]=qPart(qdd,1);
185 p1ddnorm(n)=norm(p1dd);
186 omega1x(n)=-2*ux'*EEval(p1)*p1d;
187 thetax(n)=thetax(n-1)+h*omega1x(n);
188 omega1xd(n)=-2*ux'*EEval(p1)*p1dd;
189 x2(n)=q(8);
190 x2d(n)=qd(8);
191 x2dd(n)=qdd(8);
192
193 end
Computing functions that underlie the main code outlined in Fig. 3.11.4 are identified in
Fig. 3.11.5. Computing subroutines include the Add function that enables adding nonzero
submatrices to sparse matrices, below and to the right of the address of the 1-1 term in the
submatrix added to the underlying matrix that was initialized to zeroes. Function atil evaluates
the vector product matrix of Eq. (2.1.22), ATran evaluates the orientation transformation matrix
of Eq. 2.5.22, EEval and GEval evaluate matrices E and G of Eqs. (2.6.1) and (2.6.2), and
221
GamEval evaluates the term γ of Eq. (3.1.17). Finally, BTran and KEval evaluate matrices B
and K of Eqs. (2.6.25) and (2.6.37).
Computing Functions
Add
atil
ATran
EEval
GEval
GamEval
BTran
KEval

parPart
qPart
pNormPart
Constraint and Driver Partition Functions

CylPart
DistPart
RevPart
TranPart
StrutPart
RevSphPart
UnivPart
RotDrPart
Constraint and Derivative Evaluation Functions

bbPhiDot1
bbPhiqDot1
bbP2Dot1
bbxxx
.
.
.
PhiEval
PhiqEval
P2Eval
P5Eval
Vector partition functions parPart, qPart, and xPart support partitioning of vectors
involved into components that are used in kinematic and kinetic computations. Similarly, the
constraint and driver partition functions listed provide access to constraint and driver data of the
Spatial Joint Data Table (SJDT) that is defined in the AppData function to implement
computation, as was the case for the planar code with its data table PJDT in Fig. 3.9.5.
Constraint and derivative evaluation functions listed evaluate kinematic constraint
expressions and derivatives defined in Section 3.3 that are needed for kinematic analysis. The
numerous bbxxx functions evaluate building block constraint functions and derivatives.
222
Internal details of the Functions listed in Fig. 3.11.4 are not presented, since each is
documented internally and the user need not modify these functions.
Figure 3.11.6 presents the dot1 constraint function. This building block function is in turn
used in functions PhiEval, PhiqEval, P2Eval, and P5Eval that evaluate vectors and matrices of
system constraints and their derivatives. As in Fig. 3.9.6 for the planar case, care is taken to
account for the fact that ground is designated j = 0 and its generalized coordinates are constant,
yielding no derivative contribution, but to include geometric quantities that define the constraint
of body i with ground.
1 function Phi=bbPhidot1(i,j,a1pr,a2pr,q,par)
2 [nb,ngc,nh,nhc,nd,qtol,app]=parPart(par);
3 [r1,p1]=qPart(q,i);
4 A1=ATran(p1);
5 if j==0
6 Phi=a1pr'*A1'*a2pr;
7 end
8 if j>=1
9 [r2,p2]=qPart(q,j);
10 A2=ATran(p2);
11 Phi=a1pr'*A1'*A2*a2pr;
12 end
13 end
Figure 3.11.6 Building Block Constraint Evaluation Function, bbPhidot1
3.11.3 Code Output
reports the following arrays that shed light on the application and performance of the code:
CondPhiq: the condition number of the Jacobian F q at each time step
iter; the number of Newton-Raphson iterations required at each time step
Q; the array of values of q at each time step
Qd; the array of values of q& at each time step
&& at each time step
Qdd; the array of values of q
Much as outlined in Section 3.9 for planar systems, MATLAB Code 3.11 of Appendix 3.A is a
tool for kinematic analysis of spatial systems. A general-purpose formulation and computer
implementation are especially important for spatial systems, due to the analytical complexity
associated with orientation of bodies and the resulting kinematic equations.
223
3.12 Spatial System Kinematic Analysis Using Code 3.11
Kinematic analyses of five spatial systems are carried out with MATLAB Code 3.11 to
exercise various constraint and driver formulations and study system dynamic behavior with
varying design and driver parameters. AppData sets are provided and driver definition using the
P5 function is illustrated.
3.12.1 Two Bar Mechanism with Two Rotation Drivers
The two-bar mechanism of Fig. 3.12.1 is comprised of two bars of length 1.5 m each.
Body 1 is pivoted in ground about the global z axis and body 2 is pivoted in body 1 about the
common z axis, both modeled as revolute joints. With vectors
v¢z1 = u z , v¢x1 = u x , v¢y1 = u y , v¢z2 = u z , v¢x2 = u x , and v¢y2 = u y , data that define kinematics of the
system are entered into the AppData function of Fig.3.12.2. The initial configuration is with the
bars aligned with the x axis.
Figure 3.12.1 Two Bar Mechanism with Two Rotation Drivers
11 if app==1 %2 Bar with 2 rotational drivers

12
18 nd=ngc-nc;
19
22 %k=joint No.; T=joint type(1=Dist,2=Sph,3=Cyl,4=Rev,5=Tran,
23 %6=Univ,7=Strut,8=Rev-Sph,9=DistDr,10=RotDr); i&j=bodies conn.,i>0;
24 %sipr&sjpr=vectors to Pi&Pj; d=dist.; vxipr, vzipr, vxjpr, vzjpr
25 SJDT(:,1)=[4;1;0;z3;z3;0;ux;uz;ux;uz]; %Rev-Body1 to Ground
26 SJDT(:,2)=[4;1;2;1.5*ux;z3;0;ux;uz;ux;uz]; %Tran-Body2 to Ground
27 SJDT(:,3)=[10;1;0;z3;z3;0;ux;uz;ux;uz]; %RotDr-Body 1 to ground
28 SJDT(:,4)=[10;1;2;1.5*ux;z3;0;ux;uz;ux;uz]; %RotDr-Body 1 to 2
29
224
31 r10=z3;
32 p10=[1;0;0;0];
33 r20=1.5*ux;
34 p20=[1;0;0;0];
35 q0e=[r10;p10;r20;p20];
36
37 end
Figure 3.12.2 AppData Function for 2 Bar Mechanism with 2 Rotational Drivers
Rotational drivers on body 1 relative to ground and on body 2 relative to body 1 shown in
Fig. 3.12.1 are
q1 = a ´ sin(tn)
(3.12.1)
q12 = a ´ sin(tn)
They are implemented with a = 1 in lines 14 through 19 and 62 through 66 of the P4 function of
Fig. 3.12.3. Note that the minus sign in lines 16 through 20 is required, since the rotation driver
is on body j = 0 (ground) relative to body i = 1 in Fig. 3.3.10 and this is the negative of the angle
q1 shown in Fig. 3.12.1. The remainder of the P5 function of Fig. 3.12.3 need not be modified by
the user.
Results of kinematic analysis for the x and y position, velocity, and acceleration of the
origin of the body 2 reference frame, over the period [ 0 2p] , carried out with Code 3.11 are
presented in Fig. 3.12.4.
8 if app ==1 %2 Bar with 2 rotational drivers

9
10 %Rot driver body1 to ground
11 k=3;
13
14 a1=1;
15 a=a1;
16 theta=-a*sin(tn); %Angle from ground, bod 0, to bod 1 is negative of
17 %angle from bod 1 to bod 0
18 thetad=-a*cos(tn);
19 thetadd=a*sin(tn);
20
22 A1=ATran(p1);
23
24 if j==0
28 PD=-sin(theta);
31 else
32 PD=-cos(theta);
225
35 end
36 end
37
38 if j>=1
40 A2=ATran(p2);
44 PD=-sin(theta);
47 else
48 PD=-cos(theta);
51 end
52 end
53
54 PD1=PD;
55 PDd1=PDd;
56 PDdd1=PDdd;
57
58 %Rot driver body1 to body 2
59 k=4;
61
62 a2=1;
63 a=a2;
64 theta=a*sin(tn);
65 thetad=a*cos(tn);
66 thetadd=-a*sin(tn);
67
69 A1=ATran(p1);
70
71 if j==0
75 PD=-sin(theta);
78 else
79 PD=-cos(theta);
82 end
83 end
84
85 if j>=1
87 A2=ATran(p2);
226
91 PD=-sin(theta);
94 else
95 PD=-cos(theta);
98 end
99 end
100
101 PD2=PD;
102 PDd2=PDd;
103 PDdd2=PDdd;
104
105 P=[zeros(nhc,1);PD1;PD2;zeros(nb,1)];
106 Pst=[zeros(nhc,1);PDd1;PDd2;zeros(nb,1)];
107 Pstt=[zeros(nhc,1);PDdd1;PDdd2;zeros(nb,1)];
108 end
Figure 3.12.3 P5 Time Dependent Terms, 2 Bar with 2 Rotational Drivers
Figure 3.12.4 Body 2 x and y Coordinates, Velocities, and Accelerations
227
3.12.2 Four Bar Mechanism with Internal Rotation Driver
The two-bar mechanism of Fig. 3.12.1 is modified with the addition of a distance
constraint of length 5 m between the outboard end of body 2 and the point (0,4) on the y axis, to
form the closed loop four bar mechanism that includes ground shown in Fig. 3.12.5. Data that
define the model are presented in the AppData function of Fig. 3.12.6. Since the four bar
mechanism has only one degree of freedom, a single driver is required, in this case the second
relative rotation driver between bodies 1 and 2 of Eq. (3.12.1).
Figure 3.12.5 Four Bar Mechanism with Internal Rotational Driver
40 if app==2 %4 Bar with internal rotational driver

41
47 nd=ngc-nc;
48
52 %6=Univ, 7=Strut, 8=Rev-Sph, 9=DistDr, 10=RotDr); i&j=bodies conn.,i>0;
54 SJDT(:,1)=[4;1;0;z3;z3;0;ux;uz;ux;uz]; %Rev-Body1 to Ground
55 SJDT(:,2)=[4;1;2;1.5*ux;z3;0;ux;uz;ux;uz]; %Tran-Body2 to Ground
56 SJDT(:,3)=[1;2;0;1.5*ux;4*uy;5;z3;z3;z3;z3]; %Dist-Body 1 to 2
57 SJDT(:,4)=[10;1;2;1.5*ux;z3;0;ux;uz;ux;uz]; %RotDr-Body 1 to 2
58
60 r10=z3;
61 p10=[1;0;0;0];
62 r20=1.5*ux;
63 p20=[1;0;0;0];
64 q0e=[r10;p10;r20;p20];
65
66 end
Figure 3.12.6 AppData Function for Four Bar Mechanism with Internal Rotational Driver
228
Results of kinematic analysis for position, velocity, and acceleration of the origin of the
body 2 reference frame, over the period [ 0 2p] , carried out with Code 3.11 are presented in
Fig. 3.12.7. The distinctly different characteristics of the responses shown in Figs. 3.12.4 and
3.12.7 are due to the addition of the distance constraint in Fig. 3.12.5. This is characteristic of the
distinctive difference in performance of open loop and closed loop mechanisms. While kinematic
analysis of closed loop mechanisms has traditionally been considered to be much more
complicated than for open loop mechanisms, the computational approach presented here shows
that this need not be the case.
Figure 3.12.7 Body 2 x and y Coordinates, Velocities, and Accelerations

3.12.3 Two Body Slider with Distance Driver
The mechanism shown in Fig. 3.12.8 is comprised of two bodies that translate along the x
and z axes and are connected by a distance constraint of length d = 5 m. The AppData function
that defines the kinematics of the mechanism, including a distance driver between the origin of
the global reference frame and body 1, is presented in Fig. 3.12.9.
229
Figure 3.12.8 Two Body Slider with Distance Driver on Body 1
69 if app==3 %2-body Slider, x-z plane, x1 distDr

70
76 nd=ngc-nc;
77
79 %SJTd(:,k)=[T;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr];
82 %si&jpr=vectors to Pi&j; d=dist.; vxipr, vzipr, vxjpr, vzjpr
83 SJDT(:,1)=[5;1;0;z3;z3;0;uy;ux;uy;ux]; %Tran-Body1 to Ground
84 SJDT(:,2)=[5;2;0;z3;z3;0;ux;uz;ux;uz]; %Tran-Body2 to Ground
85 SJDT(:,3)=[1;1;2;z3;z3;5;z3;z3;z3;z3]; %Dist-Body 1 to 2
86 SJDT(:,4)=[9;1;0;z3;z3;0;z3;z3;z3;z3]; %DistDr-Body 1 to Ground
87
89 r10=[3;0;0];
90 p10=[1;0;0;0];
91 r20=[0;0;4];
92 p20=[1;0;0;0];
93 q0e=[r10;p10;r20;p20];
94
95 end
Figure 3.12.9 AppData Function for Two Body Slider with Distance Driver
A distance driver from the origin of the global reference frame to the origin of the body 1
reference frame along the x axis is
d = b + a ´ sin ( t n ) (3.12.2)
as is entered in lines 160 through 168 of the P5 function of Fig. 3.12.10. For driver parameters a
= 1 and b = 3, the position of body 1 and the position, velocity, and acceleration of body 2 from
kinematic analysis with Code 3.10 are shown in Fig. 3.12.11.
230
159 if app ==3 %2-body Slider, x-z plane, distDr Bod1-grnd
160 a=1;
161 b=3;
162 %d=b+a*sin(tn)
163 PD=-0.5*(b+a*sin(tn))^2;
165 PDd=-(b+a*sin(tn))*a*cos(tn);
167 PDdd=(b+a*sin(tn))*a*sin(tn)-(a*cos(tn))^2;
169 end
Figure 3.12.10 P5 Time Dependent Terms, Two Body Slider with Distance Driver
Figure 3.12.11 Kinematic Results for Two Body Slider

3.12.4 Slider-Crank in y-z Plane
A simplified two body model of a slider-crank mechanism that functions in the y-z plane
is shown in Fig. 3.12.12. The crank, body 1, rotates about the x axis and the slider, body 2,
translates along the y axis. A point on the periphery of the crank of unit radius is connected to the
slider by a distance constraint with d = 2 m.
Figure 3.12.12 Slider-Crank in y-z Plane

The AppData function of Fig. 3.12.13 defines the model and rotation driver of the crank
relative to ground, where the initial configuration is with the connecting rod attachment point on
the crank on the global y axis. The rotation driver
231
theta = omega ´ t n
thetad = omega (3.12.3)
thetadd = 0
is implemented by entering the relations of Eq. (3.12.3) in lines 176 through 179 in the P5
function of Fig. 3.12.14.
98 if app==4 %Slider-Crank in y-z Plane, Bod 1 RotDr about x axis

99
105 nd=ngc-nc;
106
108 %SJTd(:,k)=[T;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr];
112 SJDT(:,1)=[4;1;0;z3;z3;0;uy;ux;uy;ux]; %Rev-Body1 to Ground
C:\Users\echau\Documents\CMMD\Chapter 3. Multibody System Cartesian Ki...\AppData.m Page 3
113 SJDT(:,2)=[5;2;0;z3;z3;0;uz;uy;uz;uy]; %Tran-Body2 to Ground
114 SJDT(:,3)=[1;1;2;uy;z3;2;z3;z3;z3;z3]; %Dist-Body 1 to 2
115 SJDT(:,4)=[10;1;0;z3;z3;0;uy;ux;uy;ux]; %RotDr-Body 1 to Ground
116
118 r10=[0;0;0];
119 p10=[1;0;0;0];
120 r20=[0;3;0];
121 p20=[1;0;0;0];
122 q0e=[r10;p10;r20;p20];
123
124 end
Figure 3.12.13 AppData Function for Slider-Crank in y-z Plane
172 if app ==4 %Slider-Crank in x-z Plane, Bod 1 RotDr about x axis
173 k=4;
175
176 omega=1;
177 theta=omega*tn;
178 thetad=omega;
179 thetadd=0;
180
181 [r1,p1]=qPart(q,i);
182 A1=ATran(p1);
183
184 if j==0
232
188 PD=-sin(theta);
191 else
192 PD=-cos(theta);
195 end
196 end
197
198 if j>=1
199 [r2,p2]=qPart(q,j);
200 A2=ATran(p2);
204 PD=-sin(theta);
207 else
208 PD=-cos(theta);
211 end
212 end
213
217
218 end
Figure 3.12.14 P5 Time Dependent Terms, Slider-Crank in y-z Plane
Results of kinematic analysis for position, velocity, and acceleration of the slider, over
the period [ 0 2p] , carried out with Code 3.11, are presented in Fig. 3.12.15. The linear
dependence of velocity and quadratic dependence of acceleration on omega are apparent.
233
Figure 3.12.15 Kinematic Results for Slider-Crank in y-z Plane
3.12.5 Spatial Slider-Crank
The two-body spatial slider-crank model of Section 3.3.7.2, shown here in Fig. 3.12.16,
is analyzed using Code 3.11 with data presented in Eq. (3.3.50) and incorporated in the AppData
function of Fig. 3.12.17. The radius of the crank is 0.08 m and the rotational driver imposed on
the crank is as in Eq. (3.12.3).
234
127 if app==5 %Spatial Slider-Crank, Bod 1 RotDr

128
134 nd=ngc-nc;
135
141 SJDT(:,1)=[4;1;0;z3;0.1*uy+0.12*uz;0;uz;ux;uz;ux]; %Rev-Body1to Ground
142 SJDT(:,2)=[5;2;0;z3;z3;0;uz;ux;uz;ux]; %Tran-Body2 to Ground
143 SJDT(:,3)=[1;1;2;0.08*uz;z3;0.24;z3;z3;z3;z3]; %Dist-Body 1 to 2
144 SJDT(:,4)=[10;1;0;z3;0.1*uy+0.12*uz;0;uz;ux;uz;ux]; %RotDr-Body1to Grnd
145
147 r10=[0;0.1;0.12];
148 p10=[1;0;0;0];
149 r20=[0.25;0;0];
150 p20=[1;0;0;0];
151 q0e=[r10;p10;r20;p20];
Figure 3.12.17 AppData Function for Spatial Slider-Crank
To study the effect of varying crank angular velocity, the connecting rod length is
selected as d 3 = 0.3 m. Plots of position, velocity, and acceleration (z2, z2d, and z2dd) of the
slider for crank angular velocities omega = 1, 2, and 4 rad/sec are presented in Fig. 3.12.18. As
expected, velocities vary linearly with input angular velocity and accelerations vary with the
square of velocities.
235
Figure 3.12.18 Slider Position, Velocity, and Acceleration, d 3 = 0.3m
To study the influence of variable connecting rod length, the crank is driven at
omeg = 1 rad/sec with a connecting rod length of 0.24 m. Plots of position, velocity, and
acceleration of the slider are presented in Fig. 3.12.19. Since a connecting rod length of 0.23 m
leads to a singular configuration, it is not surprising that accelerations become extreme with a
connecting rod length of 0.24 m
236
Figure 3.12.19 Slider Position, Velocity, and Acceleration, omega = 4, d 3 = 0.24m
For spatial system models with time dependent drivers that create the same number of constraint
equations as generalized coordinates, kinematic analysis is carried out using iterative methods to
solve nonlinear constraint equations for kinematically admissible configurations on a time grid
that is specified for analysis. Linear velocity and acceleration equations are solved on the same
time grid for dynamic response. Criteria for singular behavior are monitored, including the
condition number of the Jacobian, rapid changes in acceleration, and loss of existence in position
analysis.
Numerical analyses using Code 3.11 of Appendix 3.A are carried out with minimal investment of
person time, in contrast with the horror that would be associated with manual derivation and
coding. Results confirm the criteria for onset of singular behavior and quadratic dependence of
system acceleration on velocities.
237
Appendix 3.A Kinematics Code
MATLAB computer code for planar and spatial kinematic analysis is provided in software
files that accompany the text. Instruction on two substantial codes is provided in Sections 3.9 and
3.11. Codes contained in accompanying files are as follows:
Code 3.9 Planar Kinematic Analysis
Code 3.11 Spatial Kinematic Analysis
238
Appendix 3.B Key Formulas, Chapter 3
239
240
CHAPTER 4
Equations of Motion for Bodies and Multibody Systems
4.0 Introduction
Variational equations of motion that are based on Newton's laws of motion for particles,
d’Alembert’s principle, and kinematic relations defined in Chapter 3 are derived for systems of
particles in Section 4.1 and for planar and spatial rigid bodies in Sections 4.2 and 4.3. Inertia
matrices are defined for planar and spatial bodies and their properties are analyzed in Section
4.4.
External and internal forces and torques that act on and between bodies are incorporated
into the variational formulation in Section 4.5. Variational equations of motion for individual
bodies are summed over all bodies in a system in Section 4.6, to obtain multibody system
variational equations of motion. The concept of virtual work and d’Alembert’s principle, in the
form “constraint reaction forces do no work as a result of kinematically admissible virtual
displacements” is applied to obtain system variational equations of motion. Broadly applicable
conditions are defined under which the multibody system mass matrix is positive definite on the
space of kinematically admissible virtual velocities, including both holonomic and
nonholonomic constraints. This property of the system mass matrix is of critical importance in
computational dynamics. A formulation that enables initial conditions to be defined for
constrained equations of motion is presented. Examples are presented in Section 4.7, showing
that variational equations of motion for holonomic systems with independent generalized
coordinates can be reduced to second order ordinary differential equations (ODE) of motion.
Runge-Kutta numerical integration methods for first order ODE, with established
stability and accuracy properties, are presented in Section 4.8. First order ODE methods are
transformed to a form that is suitable for application to the second order ODE of dynamics.
MATLA B Code 4.8 that implements the second order solution methods is presented in
Appendix 4.A and user instruction is provided in Section 4.9.1. Numerical results for planar and
spatial examples studied in Section 4.7.1 are obtained using MATLAB Codes 4.9.1 through 4.9.3
presented in Appendix 4.A, to illustrate the use of numerical integration methods in mechanical
system dynamics.
Lagrange multipliers are introduced in Section 4.10 to evaluate constraint reaction forces
in multibody systems. This leads to equations of motion in Lagrange multipliers that appear
algebraically as unknowns in equations of motion that include algebraic kinematic constraint
equations. The resulting system has become known as differential-algebraic equations (DAE) of
motion.
241
4.1 Equations of Motion for Particles
The foundation on which the theory of mechanical system dynamics is based is three
laws of motion postulated by Newton (1687) that govern the motion of particles and
d’Alembert’s principle that states that forces of constraint do no work (d'Alembert, 1743). These
landmark contributions represent the fundamental physics that underpin a broadly applicable
formulation for dynamics of mechanical systems. Definition of position, velocity, and
acceleration of a point in space presupposes the existence of some frame of reference. For the
purposes of dynamics, it is important that the frame of reference be an inertial reference frame,
defined as a frame in which Newton's laws of motion are valid. An inertial reference frame may
be thought of as a frame fixed in some ideal space relative to which motion of stars and planets
can be described. In most engineering applications, a reference frame that is fixed to the surface
of the earth is adequate. This is technically not an inertial reference frame, due to motion of the
earth relative to the sun and other stars. Accuracy associated with an earth-fixed reference frame
is, however, acceptable for many engineering purposes.
4.1.1 Newton's Laws
In an inertial reference frame, the following are Newton's laws of motion:
First Law: A particle continues in its state of rest or constant velocity if
the resultant of forces acting on the particle is zero.
Second Law: The product of the mass of a particle and its acceleration in
an inertial reference frame is equal to the resultant force acting on the
particle.
Third Law: To every force of action on a particle there is an equal and
opposite force of reaction on another particle; i.e., the mutual forces of
interaction between two particles are equal in magnitude and opposite in
direction.
Technically, the first law may be viewed as a special case of the second law, in which a
zero resultant force implies zero acceleration, hence constant velocity. The first law, however,
plays an interesting role in static friction, or stiction.
The resultant force F(t) in the second law is the vector sum of all forces that act on a
particle. The acceleration of the particle is the second time derivative of the position vector
r = r (t) that locates the particle relative to the origin of an inertial reference frame and the mass
of the particle is denoted m. Newton's second law may thus be written in vector form as
r = F(t)
m&& (4.1.1)
In most applications, the forces of action and reaction between pairs of particles are not
only equal in magnitude and opposite in direction, as guaranteed by the third law, but they are
collinear; i.e., they act along the line between the particles. This is called the strong form of the
third law (Goldstein, 1980) and is true for gravitational forces, rectilinear spring and damper
forces, and forces of contact. Forces of interaction due to charged particles in an electric field
need not, however, be collinear.
The relation between mass m and weight w of a particle is established by the
gravitational acceleration g, which has magnitude g and is directed toward the center of the
242
earth. The weight w of the particle is a scalar quantity that is related to its mass and the
gravitational acceleration by
w = mg (4.1.2)
Units employed in defining positions of particles, forces acting on particles, and masses
of particles must be consistently defined in order that Eqs. (4.1.1) and (4.1.2) may be employed.
Standard International units (SI) of meters, newtons, and kilograms are used in the text. In all
systems of units, the unit of time is seconds.
Note that if F(t) and m are known in Eq. (4.1.1), and no constraints act on the particle,
Newton's equations of motion are second-order ordinary differential equations. As is well
known from the theory of ordinary differential equations (Teschl, 2012), initial conditions must
be specified on r and r& at an initial time t 0 to yield a unique solution of Eq. (4.1.1) for r(t),
namely
r (t 0 ) = r 0
(4.1.3)
r& (t 0 ) = r& 0
Equations (4.1.1) and (4.1.3) comprise an initial-value problem that determines the motion of a
particle.
4.1.2 Virtual Displacements
Defining the virtual displacement of a particle as a perturbation dr , its scalar product
with both sides of Eq. (4.1.1) yields
δr T ( m&&
r - F) = 0 (4.1.4)
This equation holding for arbitrary dr is equivalent to Eq. (4.1.1). It is called the variational
equation of motion for the particle.
Lagrange (1788) published a treatise on analytical dynamics that introduced concepts of
virtual displacement and virtual work associated with particles that move in space under the
action of applied forces and forces due to constraints that act between them. For a system of np
particles that are located by vectors ri , i = 1,..., np, relative to an inertial reference frame, a
vector of generalized coordinates of dimension 3np is defined as
T
q = éër1T L rnpT ùû (4.1.5)
that locates all of the particles in the inertial reference frame. As shown in Section 3.1, particles
may be subjected to kinematic constraints that specify interactions between pairs of particles,
such as massless rods that constrain the distance between particles to be constant, or actuators
that specify distances between particles as functions of time. Such constraints may be written in
equation form as holonomic constraints,
F (q, t) = 0 (4.1.6)
where F (q, t) = [ Φ1 (q, t) L Φ nh (q, t)] . While most constraints, such as those encountered in
T
Chapter 3, do not depend explicitly on time, time dependent constraints are admitted here to
account for time dependent actuators between pairs of particles. Using the vector calculus
243
notation of Eq. (2.2.35), Eq. (4.1.6) may be differentiated with respect to time to obtain the
holonomic velocity constraint equation
F q (q, t)q& = -F t (q, t) º n h (q, t) (4.1.7)
that must hold throughout the time interval of interest. In order for the constraints to be
independent, the constraint Jacobian F q (q, t) must have full rank for all q and t. Written in
differential form, Eq. (4.1.7) is
F q (q, t)dq = n h (q, t)dt (4.1.8)
As in Section 3.4, nonholonomic constraints that define nd < 3nb - nh relations among
admissible velocities can be represented in the form
E(q, t )q& = e(q, t ) (4.1.9)
The functions involved generally have one or more continuous derivatives with respect to each
of their arguments and, in order for the constraints to be independent, E(q, t) must have full rank
for all q and t. Written in differential form, this is
E(q, t )dq = e(q, t )dt (4.1.10)
If an integrating factor can be found such that its product with the left side of Eq. (4.1.10)
is a total differential, then Eq. (4.1.9) is equivalent to a holonomic constraint. If not, then Eqs.
(4.1.7) and (4.1.9) comprise a nonholonomic constraint. Necessary and sufficient conditions for
existence of an integrating factor are given in Section 2.2.6.
The system of differential constraints of Eqs. (4.1.8) and (4.1.10) may be written as
C(q, t)dq = ν (q, t)dt (4.1.11)
where
é F q (q, t )ù
C(q, t ) º ê ú
ë E(q, t ) û (4.1.12)
é ν (q, t ) ù
ν(q, t ) º ê h ú
ë e(q, t ) û
In order for the velocity constraints of Eqs. (4.1.7) and (4.1.9), equivalently Eq. (4.1.11), to be
independent, C(q, t) must have full rank for all q and t.
A differential dq that satisfies Eq. (4.1.11) is said to be an admissible displacement at
(q,t), with time increment dt. With the goal of obtaining a variational formulation of the
equations of dynamics for a system of particles that is based on Newton's equations of motion,
the use of admissible displacements is problematic. Newton's equations of motion hold at each
instant in time, they do not involve a concept of time increment. To resolve this dilemma,
Lagrange is credited (Pars, 1965) with creating the concept of a virtual displacement, denoted
dq , that is kinematically admissible at an instant in time if it satisfies Eq. (4.1.11) with dt = 0,
C(q, t)dq = 0 (4.1.13)
244
In the absence of differential constraints of the form of Eq. (4.1.9), possibly
nonholonomic, Eq. (4.1.13) reduces to F q (q, t)dq = 0 , so dq is tangent to the constraint manifold
defined in Section 3.5, for constraints of Eq. (4.1.6), with t fixed. The admissible displacement
dq of Eq. (4.1.11) is generally not tangent to the constraint manifold. It should be noted that if
the holonomic constraint of Eq. (4.1.6) does not depend explicitly on time, or if the differential
constraint of Eq. (4.1.9) does not depend explicitly on time and if e(q, t) = 0 , then there is no
distinction between admissible displacements and virtual displacements.
________________________________________________________________________
Example 4.1.1. Before proceeding to a general statement of variational equations of motion, it is
instructive to illustrate the foregoing definitions and properties with a simple example. A particle
of mass m slides without friction on a straight wire, shown as the dashed line in Fig. 4.1.1, that
moves in the plane according to the equation y = x + t . Motion of the particle must therefore
satisfy the time dependent constraint
Φ(x, y, t) = y - x - t = 0 (4.1.14)
Figure 4.1.1. Particle on Moving Wire

With q = [x y ]T , a virtual displacement dq that is consistent with the constraint
satisfies Φ q dq = [-1 1]dq = 0 , so
dq = a[1 1]T (4.1.15)

where a is arbitrary. An admissible displacement that is consistent with the constraint of Eq.
(4.1.14) satisfies
Φq dq - dt = [ -1 1]dq - dt = 0 (4.1.16)
Substitution of dq = a[1 1]T + dt[ 0 1]T into Eq. (4.1.16) yields

[1 1] ( a[1 1]T + dt[ 0 1]T ) - dt = dt - dt = 0 . Thus, with the unit vector u y = [0 1]T ,
dq = dq + dtu y (4.1.17)
satisfies Eq. (4.1.16) for arbitrary a ; i.e., it is an admissible displacement. As shown in Fig.
4.1.1, the kinematically admissible virtual displacement dq is along the wire; i.e., tangent to the
curve defined by Eq. (4.1.14). In contrast, the admissible displacement dq is not tangent to the
curve.
From the geometry of Fig. 4.1.1 and the fact that there is no friction force, the constraint
reaction force F r = g[-1 1]T must be orthogonal to the wire on which the particle slides, as
245
shown in Fig. 4.1.2. Work done by the reaction force in the kinematically admissible virtual
displacement dq of Eq. (4.1.15) is
dq T F r = a[1 1]g[-1 1]T = 0 (4.1.18)

In contrast, work done by the reaction force in the admissible displacement dq of Eq. (4.1.17) is
dqT F r = ( dqT + dt[ 0 1]) g[-1 1]T = gdt ¹ 0 (4.1.19)
This inconsistency is the reason that virtual displacements and not admissible displacements
form the foundation for variational methods in mechanics.
Figure 4.1.2. Forces Acting on Particle

_______________________________________________________________________
4.1.3 d'Alembert's Principle
Example 4.1.1 is an illustration of the basis for d'Alembert's principle (Lanczos, 1949;
Pars, 1965), which may be stated as follows:
d'Alembert's Principle: Constraint reaction forces do no work under the
action of kinematically admissible virtual displacements.
Example 4.1.1 shows that d'Alembert's Principle says nothing about work done by
constraint forces under the action of admissible displacements. Lanczos (1949) argues that while
d’Alembert’s principle appears to be related to Newton’s third law, as it involves constraint
forces of action and reaction, it cannot be deduced from the third law. It is thus accepted as a
fundamental tenant of the variational approach to mechanics. It is important to note that even if
friction acts in constraints and is influenced by normal contact forces of constraint, those
constraint forces do no work. The force of friction must be accounted for as an applied force
whose magnitude is influenced by the workless normal forces of constraint, as is done in Chapter
8. d’Alembert’s principle, together with Newton’s equations of motion, is adopted in this text as
a fundamental tenant of the variational approach to dynamics.
4.1.4 Variational Equations of Motion for Systems of Particles
Newton's equation of motion for particle i, in a collection of np particles, is
ri = Fi A + Fi C
mi&& (4.1.20)
where Fi A and Fi C are applied force and constraint reaction force that act on particle i. For an
arbitrary virtual displacement dri , Eq. (4.1.20) is equivalent to
246
driT ( m i&&
ri - Fi A - Fi C ) = 0 (4.1.21)
Using the generalized coordinates of Eq. (4.1.5) and defining

M = diag ( m1I 3 L m np I 3 )
T
F A = ëé Fi A T L FnpA T ûù (4.1.22)
T
F C = ëé Fi CT L FnpC T ûù
the sum of Eqs. (4.1.21) over all particles is

np
å dr ( m &&r - F
i =1
i
T
i i i
A
- FiC ) = dq T ( Mq
&& - F A - F C ) = 0 (4.1.23)
which must hold for arbitrary dq .

Under the condition that constraint reaction forces do no work under the action of a
kinematically admissible virtual displacement; i.e.,
dq T F C = 0 (4.1.24)
for all dq that satisfy Eq. (4.1.13), Eq. (4.1.23) reduces to
dq T ( Mq
&& - F A ) = 0 (4.1.25)
which must hold for all dq that satisfy Eq. (4.1.13). This condition is called the first form of the
fundamental equation of motion by Pars (1965) and d'Alembert's equation by others. In any case,
it is a variational equation of motion that is equivalent to Newton's equations of motion, provided
Eq. (4.1.24) holds for all dq that satisfy Eq. (4.1.13).
________________________________________________________________________
Example 4.1.2: Newton's equations of motion for the particle of Example 4.1.1 that slides
without friction on a wire, including the force of constraint, is
(
&& + mgu y - g / 2 [-1 1]T = 0
mq ) (4.1.26)
where q = [x y ]T and g is the magnitude of the constraint reaction force. The variational
equation of motion of Eq. (4.1.25), for kinematically admissible virtual displacements of Eq.
(4.1.15); i.e., dq = a[1 1]T for arbitrary α , is
dq T é mq
ë ( )
&& + mgu y - g / 2 [-1,1]T ù = a[1 1] é mq
û ë (
&& + mgu y - g / 2 [-1 1]T ù
û )
= a ëé m ( && y ) + mg ùû = 0
x + &&
Since this must hold for arbitrary a ,

( &&x + &&y ) + g = 0 (4.1.27)
247
y - &&
Taking two derivatives of the constraint equation of Eq. (4.1.14), && x = 0 . Substituting this into
Eq. (4.1.27),
x = -g / 2
&& (4.1.28)
For initial conditions at t 0 = 0 , q 0 = 0 and q& 0 = [-1 0]T that are consistent with Eq. (4.1.14) and
its time derivative y& - x& - 1 = 0 , the solution of Eq. (4.1.28) is
x& = -1 - gt/2
(4.1.29)
x = - t - gt 2 / 4
From the constraint equation of Eq. (4.1.14) and its derivatives,
y = -g / 2
&&
y& = -gt/2 (4.1.30)
y = -gt / 4
2
Equations (4.1.28), (4.1.29), and (4.1.30) comprise the solution of the variational equation of
motion.
Substituting the solution of the variational equations into Newton’s equations of motion
of Eq. (4.1.26),
é - mg / 2 + g / 2 ù
ê ú=0 (4.1.31)
ëê - mg / 2 + mg - g / 2 úû
which is satisfied with g = mg/ 2 . This verifies that the solution of the variational equation of
motion satisfies Newton's equations of motion.
________________________________________________________________________
Models of rigid bodies are next created, using collections of particles that are subjected
to distance constraints.
_______________________________________________________________________
Example 4.1.3: The pair of particles m1 ¹ 0 ¹ m 2 shown in the x-y plane in Fig. 4.1.3 is
connected by a massless bar of unit length to form a body, sometimes called a dumbbell. With
unit vector u¢x from particle one to particle two along the x ¢-axis , vectors that locate the
particles in the inertial x-y plane and give their accelerations and virtual displacements are
r1 = r
r2 = r + A (f)u¢x
r1 = &&
&& r
(4.1.32)
r + &&
r2 = &&
&& fPA (f)u¢x - f& 2 A(f)u¢x
dr1 = dr
dr2 = dr + dfPA (f)u¢x
where Eqs. (2.3.40) and (2.3.43) have been used.
248
Figure 4.1.3 Two Particle Body in x-y Plane
Expanding the sum in Eq. (4.1.25) for this system, the variational equational of motion is
dr T ( m1&& (
r - F1A ) + ( dr T + dfu¢x T A T (f)P T ) éë m 2 &&
r + && )
fPA(f)u¢x - f& 2 A (f)u¢x - F2A ùû = 0 (4.1.33)
Define
m = m1 + m 2
m2
s¢c = u¢x
m
J ¢ = m 2 u¢x Tu¢x = m 2 (4.1.34)
F = F +F
A
1
A A
2
n¢A = u¢x T A T PT (f)F2A
where s¢c locates the center of mass of the dumbbell and J¢ is its moment of inertia relative to
the origin of the x ¢-y¢ frame. Expanding Eq. (4.1.33), using Eqs. (4.1.32) and (4.1.34),
( r + m &&
dr T m&& ( ) ) (
fPA(f) - f& 2 A (f) s¢c - F A + df ms¢cT A T (f)P T&&
r + J¢&& )
f - n ¢A = 0 (4.1.35)
Since arbitrary virtual displacements and rotations of the x ¢-y¢ frame leave the unit distance
constraint between the masses unchanged, if no additional constraints act, Eq. (4.1.35) must hold
for arbitrary dr and df . Their coefficients in Eq. (4.1.35) must therefore be zero; i.e.,
r + m &&
m&& (
fPA (f) - f& 2 A(f) s¢c = F A ) (4.1.36)
r + J¢&&
ms¢cT A T (f)PT&& f = n ¢A
which are second order ODE for motion of the body defined by the pair of constrained particles.
These are a special case of planar rigid body equations of motion derived in Section 4.2.
_______________________________________________________________________
_______________________________________________________________________
Example 4.1.4: A special case of a spatial rigid body is the three particle model shown in Fig.
4.1.4, where m1 ¹ 0 is located at the origin of the x ¢-y¢-z¢ frame and m 2 ¹ 0 ¹ m3 are located on
the x ¢ and y¢ axes, one unit from the origin at the origin of the x ¢-y¢-z¢ frame. Using Eqs.
(2.4.22), (2.4.57), and (2.4.67), with unit vectors u¢x and u¢y along the x ¢ and y¢ axes, positions,
virtual displacements, and accelerations of the three particles are
249
r1 = r
r2 = r + A (p)u¢x
r3 = r + A (p)u¢y
dr1 = dr
dr2 = dr - A (p)u% ¢x dπ¢ (4.1.37)
dr3 = dr - A (p)u% ¢y dπ¢
r1 = &&
&& r
r - A (p)u% ¢x ω
r2 = &&
&& & ¢ + A (p)ω
% ¢ω
% ¢u¢x
r - A(p)u% ¢y ω
r3 = &&
&& & ¢ + A (p)ω
% ¢ω
% ¢u¢y
Figure 4.1.4 Three Particle Body in Space

Writing the sum of terms in Eq. (4.1.25) over the three particles, using Eqs (4.1.37),
expanding products, and collecting terms yields the variational equation of motion,
r - A (p) ( m 2u% ¢x + m3u% ¢y ) ω
dr T éë( m1 + m 2 + m3 ) && & ¢ + A (p)ω % ¢ ( m 2u¢x + m3u¢y ) - ( F1A + F2A + F3A ) ù
% ¢ω
û
é( m 2u% ¢x + m3u% ¢y ) A T (p)&&r - ( m 2u% ¢x u% ¢x + m3u% ¢yu% ¢y ) ω
& ¢ + m 2u% ¢xω
% ¢ω
% ¢u¢x + m3u% ¢yω % ¢u¢y ù
% ¢ω
+dπ¢ êT
ú=0
êë -u% ¢x A T (p)F2A - u% ¢y A T (p)F3A úû
(4.1.38)
Using Eqs. (2.1.26), (2.1.27), and (2.1.29),
u% ¢x ω
% ¢ω
% ¢u¢x = -u% ¢x ω
% ¢u% ¢x ω¢ = -ω
% ¢u% ¢x u% ¢x ω¢ - ω¢u¢x T u% ¢x ω¢ - u¢x ω¢T u% ¢x ω¢ = -ω
% ¢u% ¢x u% ¢x ω¢ (4.1.39)
Similarly, for the corresponding term involving u¢y , u% ¢y ω

% ¢ω
% ¢u¢y = -ω
% ¢u% ¢y u% ¢y ω¢ . Using these results
and the definitions
m º m1 + m 2 + m3
J¢ º - ( m 2 u% ¢x u% ¢x + m 3u% ¢y u% ¢y )
s¢c º (1/ m ) ( m 2u¢x + m3u¢y ) (4.1.40)
F A º F1A + F2A + F3A
n¢A º u% ¢x A T F2A + u% ¢y A T F3A
250
Eq. (4.1.38) reduces to
dr T ( m&&
r - mA(p)s%¢c ω
& ¢ + mA(p)ω % ¢s¢c - F A )
% ¢ω
(4.1.41)
+dπ¢T ( ms%¢c A T (p)&&
r + J¢ω % ¢J¢ω¢ - n¢A ) = 0
&¢+ω
Equation (4.1.41) must hold for arbitrary dr and dπ¢ , since virtual displacement and rotation of
the x ¢-y¢-z¢ frame do not change the distance between particles and no additional constraints act
on the system. Thus, the coefficients of dr and dπ¢ in Eq. (4.1.38) must be zero; i.e.,
r - mA (p)s%¢cω
m&& & ¢ + mA (p)ω
% ¢ω
% ¢s¢c = F A
(4.1.42)
ms%¢c A T (p)&&
r + J¢ω
&¢+ω
% ¢J¢ω¢ - n ¢A = 0
which are second order ODE of motion of the three particle model of a rigid body in space.
Expansion of the second of Eqs. (4.1.40) with m 2 ¹ 0 ¹ m3 yields the inertia matrix
é m3 0 0 ù
J¢ = ê 0 m2 0 ú
ê ú
êë 0 0 m 2 + m 3 úû
which is symmetric and positive definite. As will be seen in Section 4.3, Eqs. (4.1.42) are the
well-known Newton-Euler equations of motion of a rigid body in space, written in terms of
generalized coordinates of a body-fixed noncentroidal reference frame.
________________________________________________________________________
Newton’s three laws that govern the motion of particles, published in 1687, form the foundation
for dynamics of multiparticle systems. In 1788, Lagrange published work contributing the
concept of virtual displacement and associated virtual work that is the basis for the modern theory
of variational mechanics used in this text. This theory is encapsulated in the concise statement of
d'Alembert's Principle: “Constraint reaction forces do no work under the action of kinematically
admissible virtual displacements”, published by d'Alembert in 1743.
Using d’Alembert’s principle and Newton’s laws, two and three particle models of rigid planar
and spatial bodies, respectively, are formulated using kinematic relationships presented in
Chapter 3. They yield equations of motion that are identical in form to more general rigid body
models derived in Sections 4.2 and 4.3.
251
4.2 Equations of Motion for Planar Rigid Bodies
The equations of motion of a planar rigid body are derived, beginning with Newton’s
laws for a particle and a rigid body model that is comprised of a constrained system of particles.
The variational approach introduced in Section 4.1 for particles provides a self-contained
derivation of equations of motion for rigid bodies.
Consider the planar rigid body shown in Fig. 4.2.1, with a body-fixed x ¢-y¢ reference
frame that is located and oriented relative to the inertial x-y reference frame by the vector r and
angle of rotation f . A differential mass dm(P) at point P is located in the body by the vector
s P = As ¢ P , where s¢ P is fixed in the body. Forces that act on this differential mass include
external force F(P) per unit of mass dm(P) at point P and internal force f ( P,R ) per units of
mass dm(P) and dm(R) located at points P and R, as shown in Fig. 4.2.1.
Figure 4.2.1 Forces Acting on a Planar Rigid Body

As a model of a rigid body, let a distance constraint (a massless bar) act between each
pair of differential mases in the body, thought of as particles. With this model, internal forces
f(P, R) dm(R) dm(P) and f(R, P) dm(P) dm(R) of interaction on dm(P) and dm (R) act along the
massless bar between points P and R, due to the distance constraint between particles dm(P) and
dm (R), so they are equal in magnitude, opposite in direction, and collinear. While
f ( P, R ) dm ( R ) dm ( P ) = -f ( R, P ) dm ( P ) dm ( R ) follows from Newton‘s third law, Newton said
nothing about collinearity in his law of action and reaction. Collinearity of f(P, R) and f(R, P) in
the model used here for a rigid body has been called the strong form of Newton's second law
(Goldstein, 1980) and is adequate to represent internal gravitational attraction. The model used
here requires that, if forces of interaction occur in a body due to electric or magnetic field effects
that violate the collinearity assumption, they must be accounted for as external forces.
Newton’s equation of motion for differential mass dm(P) is
é ù
r P dm(P) = F ( P ) dm(P) + ê ò f ( P,R )dm(R) ú dm(P)
&& (4.2.1)
ëm û
252
where integration of the internal forces that act on dm(P) is taken over the total mass m of the
body. Equation (4.2.1) is difficult to use, since it explicitly involves the internal forces that act
within the body. Furthermore, it is written for every differential element of mass, yielding an
infinite number of equations of motion. A unifying concept that resolves this dilemma and yields
broadly applicable tools for mechanical system dynamics is the variational approach that is
presented in Section 4.1.
Let dr P denote a virtual displacement of point P; i.e., an infinitesimal variation in the
location of point P that is kinematically admissible with time held fixed, as introduced in Section
4.1. Premultiplying both sides of Eq. (4.2.1) by dr PT and integrating over the total mass m of the
body yields
ò dr
PT P
r dm(P) = ò dr PT F ( P ) dm(P) + ò ò dr PT f ( P,R )dm(R)dm(P)
&& (4.2.2)
m m mm
which must hold for arbitrary dr P . Manipulation of the double integral appearing on the right of
Eq. (4.2.2) yields
1
òm mò dr f ( P,R )dm(R)dm(P) = 2 mò mò [dr f ( P,R ) + dr f ( P,R ) ]dm(R)dm(P)
PT PT PT
1 1
= ò ò dr P Tf ( P,R )dm(R)dm(P) + ò ò dr R Tf ( R,P )dm(P)dm(R)
2 mm 2mm
(4.2.3)
where, since P and R are variables of integration, they may be renamed without affecting the
value of the integral. Furthermore, the order of carrying out the integration can be reversed
without affecting the value of the integral and f ( P, R ) dm ( R ) dm ( P ) = -f ( R, P ) dm ( P ) dm ( R )
can be used to obtain
1 1
ò ò dr f ( P,R )dm(R)dm(P) = 2 ò ò dr f ( P,R )dm(R)dm(P) - 2 ò ò dr f ( P,R )dm(R)dm(P)
PT PT RT
mm mm mm
(4.2.4)
Combining the integrals on the right and using the fact from differential calculus that
(
dr P - dr R = d r P - r R , )
ò ò δr f ( P,R ) dm(R)dm(P) = 2 ò ò δ ( r - r R ) f ( P,R ) dm(R)dm(P)
PT 1 P T
(4.2.5)
mm mm
Recall that the definition of a rigid body requires that the distance between points P and R
is constant; i.e.,
(r ) (r )
T
P
- rR P
- rR = c (4.2.6)
Taking the differential of both sides of Eq. (4.2.6), using the rules of differential calculus, and
multiplying both sides by 1/2 yields
( ) (r )
T
d rP - rR P
- rR = 0 (4.2.7)
253
( )
Since f ( P,R ) acts along the vector r P - r R , d ( r P - r R ) f ( P,R ) = 0 . Thus, the integrand on
T
the right of Eq. (4.2.5) is zero and Eq. (4.2.2) simplifies to
ò dr r dm(P) = ò dr PT F ( P ) dm(P)
&&
PT P
(4.2.8)
m m
which must hold for all dr P that are consistent with rigid-body motion.
It is important to note that while Eq. (4.2.2) holds for arbitrary dr P , Eq. (4.2.8) does not.
The price for eliminating internal forces in the second term on the right of Eq. (4.2.2) is that
dr P must be consistent with the definition of rigid-body motion. To take full advantage of Eq.
(4.2.8), the virtual displacement dr P of point P may be written in terms of variations in
generalized coordinates of the body reference frame. Using Eqs. (2.3.43) and (2.3.47),
r P = &&
&& fPA ( f ) s¢P - f& 2 A ( f ) s¢P
r + && (4.2.9)
dr P = dr + dfPA ( f ) s¢
P
(4.2.10)
Equations (4.2.9) and (4.2.10) may be substituted in Eq. (4.2.8) to obtain the variational
form of the equations of planar motion,
r ò dm(P) + dr T &&
dr T&& (
fPA (f) - f& 2 A(f) ) ò s¢ dm(P) + dfò s¢ dm(P)A T (f)PT&&
P P
T
r
m m m
&& s¢ s¢ dm(P) - dff&

+dff ò ò s¢ A (f)P A (f)s¢ dm(P)
P P P P
T 2 T T T
(4.2.11)
m m
= dr T ò F(P)dm(P) + df ò s¢ T A T (f)P T F(P)dm(P)

P
m m
where terms that do not depend on location within the body are taken outside integrals. While
Eq. (4.2.8) holds only for dr P that are consistent with the definition of rigid-body motion, Eq.
(4.2.11) holds for any dr and df that are consistent with external constraints that act on the
body. This is true since dr P given by Eq. (4.2.10) is consistent with Eq. (4.2.7), which defines
rigid-body motion.
The form of the equations of motion of a planar rigid body in Eq. (4.2.11) can be
substantially simplified. The first integral on the left is the total mass of the body; i.e.,
m = ò dm ( P ) (4.2.12)
m
By definition of centroid, the vector from the origin of the x ¢-y¢ frame to the centroid is
1
s¢c º ò s¢ dm(P)
P
(4.2.13)
mm
and the polar moment of inertia of the body relative to the origin of the x ¢-y¢ frame is
J¢ º ò s¢ Ts¢ dm(P)
P P
(4.2.14)
m
254
Using PT = -P and Eq. (2.3.33), s¢ T A T (f)P T A (f)s¢ = ( Ps P ) s P = 0 , since Ps P is
P P T
orthogonal to sP . Thus, the last term on the left of Eq. (4.2.11) is

ò ¢ ¢
P P
s A
T T
(f) P T
A (f)s dm(P) = 0 . The first integral on the right of Eq. (4.2.11) is the total external
m
force that acts on the body,
F º ò F ( P )dm(P) (4.2.15)
m
and the second integral is the total external torque that acts about the origin of the x ¢-y¢ frame,
due to the distributed applied force; i.e.,
n¢ º ò éë -s Py¢ Fx ¢ ( P ) + s Px ¢ Fy ¢ ( P ) ùûdm(P)
m
( ) F¢ ( P )dm(P) = ò s¢ T P T A T (f)F ( P )dm(P)

T
= ò Ps¢
P P
(4.2.16)
m m
= ò s¢ T A T (f)PT F ( P )dm(P)
P
where A T (f)P T = P T A T (f) from Eq. (2.3.33) has been used.

If F(P) = ga is the gravitational force in the direction defined by unit vector a, then Eqs.
(4.2.15) and (4.2.16) yield
Fg = ò gadm(P) = ga ò dm(P) = mga
m m
(4.3.17)
n ¢g = ò s¢PT A T (f)P Tgadm(P) = ò s¢PTdm(P)A T (f)P Tga = mgs¢c T A T (f)PTa
m m
Substituting the foregoing results into Eq. (4.2.11) yields the noncentroidal planar
variational equation of motion,
dr T ëé m&& (
r + m && )
fPA (f) - f& 2 A(f) s¢c - F ûù + df ëé ms¢cT A T (f)P T&&
r + &&
fJ¢ - n¢ ûù = 0 (4.2.18)
T
In terms of body generalized coordinates q = éër T fùû , this is
dq T [ Mq
&& - S - Q ] = 0 (4.2.19)
where
é mI mPA(f)s¢c ù
M = ê cT T ú
ë ms¢ A (f)P J¢
T
û
éf& 2 mA(f)s¢c ù
S=ê ú (4.2.20)
ë 0 û
éFù
Q=ê ú
ë n ¢û
255
If the origin of the x ¢-y¢ frame is selected to be at the centroid of the body; i.e., s¢c = 0 ,
Eq. (4.2.18) reduces to the centroidal planar variational equation of motion,
dr T [ m&&
r - F ] + df ëé&&
fJ¢ - n¢ ûù = 0 (4.2.21)
In matrix form, this is the same as Eqs. (4.2.19) and (4.2.20), but with M diagonal and S = 0 ,
since s¢c = 0 .
If no external constraints act on the body, dr and df are arbitrary, so their coefficients in
Eqs. (4.2.18) and (4.2.21) must be zero. This yields the planar body differential equations of
motion
r + m &&
m&& (
fPA (f) - f& 2 A (f) s¢c = F ) (4.2.22)
cT T
r + &&
ms¢ A (f)P && fJ ¢ = n¢
T
for a noncentroidal reference frame, and

r=F
m&&
(4.2.23)
J¢&&
f = n¢
for a centroidal reference frame. Note that Eq. (4.2.22) is exactly the same as Eq. (4.1.35) in
Example 4.1.3 for motion of a rigid body that is made up of two particles connected by a single
distance constraint.
It is important to remember that Eqs. (4.2.22) and (4.2.23) are valid only if the force
vector F and the torque n ¢ that act on the body represent all forces and torques that act on the
body. A manipulation identical to that carried out in obtaining Eq. (4.2.16) shows that the
counterclockwise torque about the origin of the x ¢-y¢ frame due to a force FP applied at point P
in the x ¢-y¢ frame, located by the vector s¢P is given by
n ¢ = s¢PT A T (f)P T F P = s¢PT PT A T (f)F P = s ¢PT P T F¢P (4.2.24)

Since it is clear that the centroidal formulation of the equations of motion is simpler than
the noncentroidal formulation, one might ask; why would the noncentroidal formulation ever be
used? One reason is that analysis of the effects of design variation requires use of a
noncentroidal formulation. If b = [ b1 L b k ] is a vector of design variables, the location of
T
the centroid will almost always be a function of design; i.e., s¢c = s¢c ( b ) . If a centroidal reference
¶s¢c 0
0 c
( )
frame is selected for a nominal design b ; i.e., s¢ b = 0 , in general, 0
¶b
( )
b ¹ 0 and design
sensitivity analysis will be in error if the centroidal form of the equations of motion is used.
Variational and differential equations of motion for a planar rigid body are derived for a body that
is modeled as a constrained system of particles, using only d’Alembert’s principle and Newton’s
laws. This is with the fact that the strong form of Newton’s third law is valid; i.e., constraint
forces that act on particles that are connected by a massless rod are equal in magnitude, opposite
in direction, and collinear.
256
Key Formulas
dr T ëé m&& (
r + m && )
fPA (f) - f& 2 A(f) s¢c - F ûù + df ëé ms¢cT A T (f)P T&&
r + &&
fJ¢ - n¢ ûù = 0 (4.2.18)
dr T [ m&&
r - F ] + df ëé&&
fJ¢ - n¢ ûù = 0 (4.2.21)
r + m &&
m&& ( )
fPA(f) - f& 2 A(f) s¢c = F r + &&
ms¢cT A T (f)PT&& fJ ¢ = n ¢ (4.2.22)
r=F
m&& J ¢&&
f = n¢ (4.2.23)
257
4.3 Equations of Motion for Spatial Rigid Bodies
Consider the spatial rigid body shown in Fig. 4.3.1 that is located in space by a vector r
and Euler parameter generalized coordinates that define orientation of the x ¢-y ¢-z ¢ body-fixed
reference frame, relative to an x-y-z inertial reference frame. The spatial formulation of
equations of motion presented here is almost identical to that presented in Example 4.1.4 for
constrained particles in space and in Section 4.2 for planar bodies, prior to the point at which
angular velocity is introduced.
4.3.1 Angular Velocity Form of Equations of Motion
A differential mass dm(P) at a typical point P in the rigid body is located by vector
s = A(p )s¢P , where s¢ P is fixed in the body. As a model of a rigid body, let a distance constraint
P
act between pairs of differential masses in the body, as in Fig. 4.3.1.
Figure 4.3.1 Forces Acting on a Rigid Body in Space

Forces that act on a differential mass at point P include the external force F(P) per unit of
mass at point P and the internal force f(P, R) per units of masses dm(P) and dm(R) at points P
and R, as shown in Fig. 4.3.1. Internal forces in this model of a rigid body are due only to
gravitational interaction and distance constraints. By Newton's third law, reaction forces acting
on dm(P) and dm(R) are equal in magnitude and opposite in direction; i.e.,
f (P, R) dm ( P ) dm ( R ) = -f (R, P) dm ( P ) dm ( R ) (4.3.1)
In general, as noted in Section 4.1, Newton's third law does not require reaction forces to be
collinear. For gravitational interaction and constraint forces due to a distance constraint,
however, they are collinear.
258
Newton’s equation of motion for differential mass dm(P) is
æ ö
r P dm ( P ) = F ( P ) dm ( P ) + ç ò f (P, R) dm ( R ) ÷ dm ( P )
&& (4.3.2)
èm ø
where integration of the internal force f(P, R) with respect to differential mass at point R is taken
over the entire body. Let δr P denote an arbitrary infinitesimal variation, or virtual displacement,
T
of point P. Multiplying both sides of Eq. (4.3.2) on the left by δr P and integrating over the total
mass of the body yields
ò δr r dm ( P ) = ò δr PT F ( P ) dm ( P ) + ò ò δr PT f ( P,R ) dm ( R )dm ( P )
&&
PT P
(4.3.3)
m m mm
T
where δr P may be taken inside the integral on the right of Eq. (4.3.2), since it is constant
relative to integration over mass distributed at points R.
An argument identical to that presented in Section 4.2 shows that the double integral on
the right of Eq. (4.3.3) is zero. Thus, Eq. (4.3.3) reduces to
ò δr r dm ( P ) = ò δr P F ( P ) dm ( P )
PT P T
&& (4.3.4)
m m
Using the acceleration relation of Eq. (2.4.53) and the variational relation of Eq. (2.4.63), with
the Euler parameter form of the orientation transformation matrix, Eq. (4.3.4) may be expanded
to obtain
ò δr r dm ( P ) = ò ( δr - A(p)s%¢P δπ¢ ) && (

%& ¢s¢P + A(p)ω % ¢s¢P dm ( P ) )
T
&&
PT P
r + A (p)ω % ¢ω
m m
æ ö
r ò dm ( P ) + A (p)ω
= δr T ç && ( %& ¢ + A (p)ω % ¢ω )
% ¢ ò s¢P dm ( P ) ÷ (4.3.5)
è m m ø
æ ö
+ δπ¢T ç ò s%¢P dm ( P ) A T (p)&& &% ¢s¢P dm ( P ) + ò s%¢Pω
r + ò s%¢P ω % ¢s¢P dm ( P ) ÷
% ¢ω
èm m m ø
( )
T
where use has been made of the fact that s% ¢ P = -s% ¢ P and quantities that do not depend on the
location of point P and which appear as left or right factors in an integrand have been moved
outside the integral. Using vector identities w%& ¢s ¢ P = - s% ¢ P w& ¢ and s% ¢ P w% ¢ = w% ¢s% ¢ P + w ¢s ¢ P - s ¢ P w ¢ T of
T
Eqs. (2.1.27) and (2.1.33), the last two terms on the right of Eq. (4.3.5) may be written as
æ %& ¢s¢Pdm ( P ) + s%¢P w ö
δp ¢T ç ò s%¢Pw ò % ¢s¢Pdm ( P ) ÷
% ¢w
èm m ø
æ ö
= -δp ¢T ç ò s%¢Ps%¢Pdm ( P )w % ¢ò s% ¢P s% ¢Pdm ( P ) w¢ ÷
&¢+w
èm m ø
T
where s ¢ P s% ¢ P = 0 = w ¢ T w% ¢ , from Eq. (2.1.27), have been used. Substituting this result into Eq.
(4.3.5) yields
259
æ ö
ò δr r dm ( P ) = δr T ç &&
&&
PT P
r ò dm ( P ) + A(p)w (
%& ¢ + A (p)w % ¢ ò s¢P dm ( P ) ÷
% ¢w )
m è m m ø
(4.3.6)
æ ö
+ δp¢T ç ò s%¢P dm ( P ) A T (p)&&
r - ò s%¢P s%¢P dm ( P )w
& ¢-w% ¢ ò s%¢P s%¢P dm ( P ) w¢ ÷
èm m m ø
Similarly, using Eq. (2.4.63), the right side of Eq. (4.3.4) may be written as
ò δr
PT
F ( P ) dm ( P ) = ò ( δr T + δp¢T s%¢P A T (p) ) F ( P ) dm ( P )
m m
= δr ò F ( P ) dm ( P ) + δp¢T ò s%¢P A T (p)F ( P ) dm ( P ) (4.3.7)

m m
= δr ò F ( P ) dm ( P ) + δp¢T ò s%¢P F¢ ( P ) dm ( P )
m m
Substituting from Eqs. (4.3.6) and (4.3.7) into Eq. (4.3.4) and collecting terms,
æ ö
r ò dm ( P ) + A(p) w
δr T ç && (
%& ¢ + w
% ¢w )
% ¢ ò s¢P dm ( P ) - ò F ( P ) dm ( P ) ÷
è m m m ø
(4.3.8)
æ ö
+δp¢T ç ò s%¢P dm ( P ) A T (p)&&r - ò s%¢P s%¢P dm ( P )w
&¢-w% ¢ ò s%¢P s%¢P dm ( P ) w¢ - ò s%¢P F¢ ( P ) dm ( P ) ÷ = 0
èm m m m ø
Terms in Eq. (4.3.8) have both physical and mathematical significance,
m º ò dm ( P ) (4.3.9)
m
is the total mass of the body,

1
s¢P dm ( P )
m mò
s¢c º (4.3.10)
is the vector from the origin of the body-fixed x ¢-y ¢-z ¢ frame to the centroid, or center of mass,
of the body,
J ¢ = - ò s% ¢ Ps% ¢ P dm ( P )
m
( ) ( )
é y ¢P 2 + z ¢P ù
2
- x ¢P y¢ P - x ¢ P z ¢P
ê ú (4.3.11)
= ò m êê - x ¢ P y ¢ P ( x¢ ) + ( z¢ )
P 2 P 2
- y¢ z ¢
P P ú
údm ( P )
ê 2ú
( ) ( )
2
êë - x ¢ z ¢ - y¢ P z ¢P x ¢ P + y ¢P ú
P P
û
is the inertia tensor, or inertia matrix, of the body relative to the x ¢-y ¢-z ¢ frame,
F º ò F ( P )dm ( P ) (4.3.12)
m
260
is the resultant force that acts at the origin of the body-fixed x ¢-y ¢-z ¢ frame, represented in the
x-y-z frame, and
n¢ º ò s%¢P F¢ ( P ) dm ( P ) (4.3.13)
m
is the resultant torque that acts on the body about the origin of the x ¢-y ¢-z ¢ frame, represented in
that frame.
If F(P) = ga is the gravitational force in the direction defined by unit vector a, then Eqs.
(4.3.12) and (4.3.13) are
Fg = ò gadm(P) = ga ò dm(P) = mga
m m
(4.3.14)
n¢g = ò s%¢ A (p )gadm(P) = ò s% ¢Pdm(P)A T (p )ga = mgs%¢c A T (p )a
P T
m m
²
ò s% ¢ dm(P) = ò s%¢ dm(P) = ms% ¢ , Eq.
P P c
In terms of the foregoing quantities, and noting that
m m
(4.3.8) may be written as
δr T ( m&&
r - mA (p)s%¢c w
& ¢ + mA(p)w % ¢s¢c - F )
% ¢w
(4.3.15)
+ δp¢T ( ms%¢c A T (p)&&
r + J ¢w % ¢J ¢w¢ - n¢ ) = 0
&¢+w
This is the noncentroidal variational equation of motion of the body.

If no external constraints act on the body, Eq. (4.3.15) must hold for arbitrary virtual
displacement δr and virtual rotation δp¢ . Thus, the coefficients of these vectors in Eq. (4.3.15)
must be zero, yielding the classical noncentroidal Newton-Euler equations of motion (Pars,
1965),
r - mA (p)s%¢cw
m&& & ¢ + mA(p)w
% ¢w
% ¢s¢c = F
(4.3.16)
ms%¢c A T&&
r + J¢w
&¢+w
% ¢J¢w¢ = n¢
It is interesting to note that Eq. (4.3.16) is exactly the same as Eq. (4.1.41) in Example 4.1.4, in
which equations of motion of a three particle model of a rigid body were derived.
It is common in applications to select a body-fixed x ¢-y ¢-z ¢ reference frame whose origin
is at the center of mass, or centroid. In this case, s ¢ c = 0 and the variational equations of motion
of Eq. (4.3.15) simplify to
δr T ( m&&
r - F ) + δπ¢T ( J ¢ω % ¢J ¢ω¢ - n¢) = 0
&¢+ω (4.3.17)
If no external constraints act on the body, this yields the centroidal Newton-Euler equations of
motion
mr&& = F
(4.3.18)
J¢w&¢+w % ¢J¢w¢ = n¢
261
While the simplified form of the centroidal equations of motion is aesthetically appealing
and reduces computational complexity by eliminating three terms, it does not explicitly account
for the effect of a noncentroidal body-fixed x ¢-y ¢-z ¢ reference frame. This is critical when
dealing with design sensitivity analysis, as explained at the end of Section 4.2. While the
simplified forms of centroidal equations of motion yield some computational benefit, in order to
use them, modeling of bodies must be done in a centroidal reference frame, often complicating
matters. With available computing power, the penalty in using noncentroidal equations of motion
is, in fact, minimal.
4.3.2 Euler Parameter Form of Equations of Motion
Identities of Eqs. (2.6.77), (2.6.68), (2.6.63), (2.6.15), and (2.4.43) yield
& ¢ = 2Gp
w &&
dp¢ = 2Gdp
(4.3.19)
&
w¢ = 2Gp& = -2Gp
& = -A
% ¢ = AT A
w & TA
where G º G(p), G & º G(p)

& , and A º A(p) . These relations may be used to transform the
equations of motion of Section 4.3.1 to Euler parameter form.
Substituting the identities of Eq. (4.3.19) into the variational equation of motion of Eq.
(4.3.15) yields
dr T éë m&&
r - 2mAs%¢cGp & & T As¢c - F ù
&& - mAA û
(4.3.20)
+2dp TG T éë ms%¢c A T&&
r + 2J¢Gp & ¢Gp
&& - 2A T AJ & - n¢ ù = 0
û
which must hold for all δr and δp for which p T δp = 0 . This requirement is imposed so that the
transformations of Eqs. (4.3.19) are valid. Using Eq. (2.6.54) and Euler parameter identities of
Eqs. (2.6.9) and (2.6.10) that hold provided the Euler parameter normalization constraint is
satisfied,
& ¢Gp
4G T A T AJ & = 4G T GET 2EG
& T J¢Gp
& ( )
(
= 8 I - pp T )( I - pp ) G& J¢Gp
T & T
= 8 ( I - 2pp + p ( p p ) p ) G
T T & J ¢Gp
T& T
& T J ¢Gp& - 8pp TG

= 8G & T J ¢Gp
&
and
& & T As¢c = 2EG
& T 2EG
&T ( )
T
AA EG Ts¢c
& TG
= 4EG (
& I - pp T G Ts¢c )
& TGG
= 4EG & Ts¢c
Substituting these relations into Eq. (4.3.20),
262
dr T ëé m&&
r - 2mAs%¢cGp & T GG
&& - m4EG & Ts¢c - F ù
û
(4.3.21)
+dpT éë 2mG T s%¢c A T&&
r + 4G T J ¢Gp & T J ¢Gp
&& - 8G & + 8pp TG
& T J ¢Gp
& - 2G T n¢ù = 0
û
Since δp Tp = 0 , this reduces to
dr T ëé m&&
r - 2mAs%¢cGp & TGG
&& - 4mEG & Ts¢c - F ù
û
(4.3.22)
+dpT éë 2mG T s%¢c A T&&
r + 4G T J¢Gp & T J ¢Gp
&& - 8G & - 2G Tn¢ ù = 0
û
where F is the resultant force acting at the origin of the body-fixed x ¢-y ¢-z ¢ frame, represented
in the x-y-z frame, and n¢ is the resultant torque acting on the body about the origin of
the x ¢-y ¢-z ¢ frame, represented in that frame. Since gravitational force mga in direction a,
represented in the x-y-z frame, acts at the centroid, the gravitational torque mgs%¢c A T a must be
included in n¢ .
For a centroidal body fixed reference frame, s¢c = 0 and Eq. (4.3.22) reduces to
dr T [ m&&
r - F ] + dpT ëé 4G T J ¢Gp & T J ¢Gp
&& - 8G & - 2G Tn¢ù = 0
û (4.3.23)
In terms of Cartesian generalized coordinates q = [r T p T ]T , the noncentroidal variational

equation of Eq. (4.3.22) may be written as
dq T {Mq
&& -S - Q} = 0 (4.3.24)
T
which must hold for all δq = éëδr T δp T ùû that satisfy p T δp = 0 , where
é mI -2mAs%¢cG ù
Mºê ú
ë 2mG s%¢ A 4G T J ¢G û
T c T
é 4mEG & TGG& Ts¢c ù

Sºê ú (4.3.25)
ë 8G & T J ¢Gp
& û
é F ù
Qºê T ú
ë 2G n¢û
With a centroidal body fixed reference frame, Eq. (4.3.24) reduces to
dq T {Mq
&& -S - Q} = 0 (4.3.26)
T
which must hold for all δq = éëδr T δp T ùû that satisfy p T δp = 0 , where
é mI 0 ù
M=ê ú
ë 0 4G J¢G û
T
(4.3.27)
é 0 ù
S=ê &T & ú
ë8G J ¢Gp û
263
In either case, Eq. (4.3.24) or Eq. (4.3.26) may be written in the form
dqT [ Mq
&& - S - Q] = 0 (4.3.28)
T
which must hold for all δq = éëδr T δp T ùû that satisfy p T δp = 0 .
It is important to recall that the generalized force Q that acts on the body includes both
externally applied forces and torques and constraint forces and torques, if any external
constraints act on the body. Further, since δq in Eqs. (4.3.24), (4.3.26), and (4.3.28) is not
arbitrary, it is not possible to obtain differential equations of motion, as was possible in Eqs.
(4.3.16) and (4.3.18) in angular velocity generalized coordinates. Since Eq. (4.3.28) holds for all
T
δq = éëδr T δp T ùû that satisfy p T δp = δp Tp = 0 , the Lagrange Multiplier Theorem of Section
2.2.5 implies the existence of a scalar λ such that
dqT [ Mq
&& + pλ - S - Q ] = 0
for all δq , so
&& + pλ = S + Q
Mq (4.3.29)
This is a special case of more general differential-algebraic equations (DAE) that will be
encountered in Section 4.10. It is called a DAE because the Lagrange multiplier appears
algebraically and the algebraic constraint p Tp = 1 must be satisfied. Equation (4.3.29) is thus not
an ODE.
Equations of motion for a spatial rigid body are derived for a model that is based on a constrained
system of particles, using only d’Alembert’s principle and Newton’s laws, with the “strong form
of Newton’s third law”, leading to both variational and differential equations of motion in angular
velocities. As shown in Section 2.6.5, however, angular velocity is not the derivative of anything,
so equations of motion in angular velocity are not second order ODE.
Transforming the variational equation of motion to Euler parameter form yields a variational
equation that cannot be reduced to an ODE, because dp must satisfy p T dp = 0 . While the
equations of motion can then be written as a DAE, it is not an ODE. dp must satisfy p T dp = 0 .
While the equations of motion can then be written as a DAE, it is not an ODE.
Key Formulas
δr T ( m&&
r - mA (p)s%¢cw
& ¢ + mA(p)w % ¢s¢c - F )
% ¢w
(4.3.15)
+ δp¢T ( ms%¢c A T (p)&&
r + J¢w % ¢J¢w¢ - n¢ ) = 0
&¢+w
δr T ( m&&
r - F ) + δp¢T ( J ¢w
&¢+w % ¢J¢w¢ - n¢ ) = 0 (4.3.17)
dr T ëé m&&
r - 2mAs%¢cGp && - 4mEG & TGG
& T s¢c - F ù
û
(4.3.22)
+ dp éë 2mG s%¢ A &&
T T c T
r + 4G J ¢Gp
T & T J ¢Gp
&& - 8G & - 2G Tn¢ù = 0
û
dr [ m&&
T
r - F ] + dp ëé 4G J ¢Gp
T T & T &
&& - 8G J ¢Gp - 2G n¢ûù = 0
T
(4.3.23)
264
4.4 Centroid and Moments and Products of Inertia
Inertia properties of a spatial rigid body are derived, including transformation of the
inertia matrix from an arbitrary body fixed reference frame to a centroidal frame. Relations that
enable location of the centroid and evaluation of the inertia matrix for composite bodies are
derived. Transformation of the polar moment of inertia from an arbitrary body fixed frame to a
centroidal frame for planar bodies is obtained as a special case.
4.4.1 Location of Centroid
To find the centroid of a spatial body, let a noncentroidal body-fixed x ¢¢-y¢¢-z¢¢ reference
frame with origin O¢¢ be given. Let the centroid be designated as point O¢ and let the origin of
the x ¢-y¢-z¢ frame be O¢ , as shown in Fig. 4.4.1.
Figure 4.4.1 Location of Centroid

The vector ρ¢¢ in the x ¢¢-y¢¢-z¢¢ frame that locates the centroid O¢ may be determined by
writing the vector to point P in the x ¢-y¢-z¢ frame as
P
(
s¢ = C s¢¢ - ρ¢¢
P
) (4.4.1)
( )
where s¢ = C s¢¢ - ρ¢¢ emanates from the origin of the x ¢-y¢-z¢ frame and C is the constant
P P
orthogonal transformation matrix from the x ¢¢-y¢¢-z¢¢ frame to the x ¢-y¢-z¢ frame; i.e., s¢ = Cs¢¢ .
Note that s¢P ¹ Cs¢¢P , since the vectors are not defined in reference frames with a common origin.
Using Eq. (4.3.10),
265
m
P
m
(
0 = ò s¢ dm(P) = ò C s¢¢ - ρ¢¢ dm(P)
P
)
=C ò s¢¢ dm(P) - Cρ¢¢ò dm(P) = C ò s¢¢ dm(P) - mCρ¢¢
P P
m m m
Multiplying both sides of this equation on the left by C = C-1 , T
1
ρ¢¢ = ò s¢¢ dm(P)
P
(4.4.2)
mm
Consider the body shown in Fig. 4.4.2, for which the location of points and mass
distribution are symmetric about a plane with normal n. Select a body x ¢¢-y¢¢-z¢¢ frame so that its
origin is on the plane of symmetry. Thus, for points P + and P - and associated differential masses
that are symmetrically placed with respect to the plane of symmetry,
P- P+
n Ts¢¢ = -n Ts¢¢
(4.4.3)
dm(P - ) = dm(P + )
Figure 4.4.2 Body with Plane of Symmetry

Taking the scalar product of both sides of Eq. (4.4.2) with n and using Eq. (4.4.3) to change
variables of integration,
1
m mò
n Tρ¢¢ = n s ¢¢ dm(P)
T P
1 1
ò ò
T P+ T P-
= n s ¢¢ dm(P +
) + n s¢¢ dm(P- ) (4.4.4)
m m+ m m-
=
1
ò
m m+
1
(
nTs¢¢P dm(P+ ) + ò n T -s¢¢P dm(P+ ) = 0
+
m m+
+
)
Equation (4.4.4) shows that if a body has a plane of symmetry, then the centroid lies on that
plane. If a body has an axis of symmetry; i.e., every plane that contains the axis of symmetry is a
plane of symmetry, then the centroid lies on the axis of symmetry. This is the case for bodies of
revolution that may be formed on a lathe. Note that if a body is geometrically symmetric, but is
asymmetrically nonhomogeneous, these helpful geometric results are no longer valid.
266
4.4.2 Transformation of Inertia Matrices
The inertia matrix of Eq. (4.3.11) relative to the x ¢¢-y¢¢-z¢¢ frame in Fig. 4.4.1 is
J¢¢ = - ò s%¢¢ s%¢¢ dm(P)

P P
(4.4.5)
m
The transpose of the inertia matrix is
( ) dm(P) = - ò ( s%¢¢ )dm(P) = - ò ( (-s%¢¢ )(-s%¢¢ ) )dm(P) = -ò (s%¢¢ s%¢¢ )dm(P)

T
J¢¢T = - ò s%¢¢ s%¢¢ s%¢¢
P P P P P P P P
T T
m m m m
so it is symmetric. If there are three elements of nonzero mass in the body that are noncoincident
and not on a straight line, as shown in Example 4.1.4, J¢¢ is positive definite. A more general
criteria for positive definiteness is presented in what follows.
To obtain a relationship between J¢¢ and the inertia matrix J¢ with respect to the
x ¢-y¢-z¢ centroidal reference frame, with s¢¢ = ρ¢¢ + CTs¢ , first let a¢ = s¢P and a¢¢ = s¢¢P - ρ¢¢ ,
P P
where a¢ = Ca¢¢ . Applying Eq. (2.4.27),

²
s% ¢P = a% ¢ = Ca% ¢¢CT = C(s ¢¢P - ρ¢¢)CT = Cs%¢¢P CT - Cρ% ¢¢CT (4.4.6)
Thus,
s%¢¢ = ρ% ¢¢ + CT s%¢ C
P P
(4.4.7)
Substituting this into Eq. (4.4.5),
( )(
J¢¢ = - ò ρ% ¢¢ + CT s%¢ C ρ% ¢¢ + CT s%¢ C dm(P)
m
P P
)
(4.4.8)
= - mρ% ¢¢ρ% ¢¢ - ρ% ¢¢CT ò s%¢ dm(P)C - CT ò s%¢ dm(P)Cρ% ¢¢ - CT ò s%¢ s%¢ dm(P)C
P P P P
m m m
Since the x ¢-y¢-z¢ frame is centroidal, ò s%¢ dm(P) = 0 and Eq. (4.4.8) reduces to
P
J¢¢ = CT J ¢C - mρ% ¢¢ρ% ¢¢ (4.4.9)

Using Eq. (2.1.30), Eq. (4.4.9) may be written in the more conventional form,
J¢¢ = CT J ¢C + m ρ¢¢Tρ¢¢I - ρ¢¢ρ¢¢T ( ) (4.4.10)
In the special case C = I; i.e., the x ¢-y¢-z¢ and x ¢¢-y¢¢-z¢¢ frames are parallel, moments of
inertia on the diagonal of Eq. (4.4.10) are related by
(
J x ¢¢x ¢¢ =J x¢x ¢ +m ρ2y¢¢ +ρ z2¢¢ )
J y ¢¢y ¢¢ =J y ¢y ¢ +m ( ρ 2
x¢¢ +ρ z2¢¢ ) (4.4.11)
J z¢¢z¢¢ =J z¢z¢ +m ( ρ 2
x¢¢ +ρ 2y ¢¢ )
Thus, moments of inertia with respect to a noncentroidal x ¢¢-y¢¢-z¢¢ frame are those with respect to
the parallel centroidal x ¢-y¢-z¢ frame plus the mass of the body times the square of the distances
267
between the respective parallel axes. This is the so-called parallel axis theorem. For parallel
axes, products of inertia are obtained from off-diagonal terms in Eq. (4.4.10) as
J x ¢¢y¢¢ =J x¢y¢ - mρ x ¢¢ρy ¢¢
J x ¢¢z¢¢ =J x ¢z¢ - mρ x ¢¢ρ z¢¢ (4.4.12)
J y¢¢z¢¢ =J y¢z¢ - mρ y ¢¢ρ z¢¢
Consider a special case in which one of the planes of an x ¢¢-y¢¢-z¢¢ frame is a plane of
symmetry; e.g., the x ¢¢-y¢¢ plane. Then, using the symmetry argument employed in deriving Eq.
(4.4.4),
J z¢¢x ¢¢ = J z¢¢y¢¢ = 0 (4.4.13)
Similarly, if the x ¢¢-z¢¢ plane is a plane of symmetry,

J x ¢¢y¢¢ = J z¢¢y¢¢ = 0 (4.4.14)
and if the y¢¢-z¢¢ plane is a plane of symmetry,
J x ¢¢y¢¢ = J x ¢¢z¢¢ = 0 (4.4.15)
If any two coordinate planes of the x ¢¢-y¢¢-z¢¢ frame are planes of symmetry; i.e., have both
geometric and mass distribution symmetry, then from Eqs. (4.4.13) to (4.4.15), all products of
inertia are zero. This is a very helpful property, since bodies in mechanical systems often have
two planes of symmetry. A common case of pairs of planes of symmetry is a homogeneous body
of revolution about one of the axes of a reference frame.
4.4.3 Principal Axes
It is possible to find a centroidal reference frame with respect to which the products of
inertia are all zero. Let the x ¢¢-y¢¢-z¢¢ and x ¢-y¢-z¢ frames have their origins at the centroid of a
body. Then, ρ¢¢ = 0 . Presume J¢¢ is known, in general as a full symmetric matrix. Multiplying
Eq. (4.4.9) on the left by C and on the right by CT ,
J¢ = CJ¢¢CT (4.4.16)
The task is now to find an orthogonal transformation matrix C from the x ¢¢-y¢¢-z¢¢ frame to the
x ¢-y¢-z¢ frame for which J¢ is diagonal.
From the definition of the symmetric matrix J¢¢ in Eq. (4.4.5), for any vector a,
a T J ¢¢a = - ò a Ts%¢¢s% ¢¢adm(P) = ò aTs%¢¢Ts% ¢¢adm(P)
m m
(4.4.17)
= ò ( s%¢¢a ) ( s% ¢¢a ) dm(P) ³ 0
T
Thus, J¢¢ is positive semidefinite. In fact, providing that mass density is nowhere zero in the body,
a T J¢¢a = 0 implies that s%¢¢a = 0 = -as
% ¢¢ , for all s¢¢ in the body. If there are three linearly
independent vectors s¢¢i , i = 1, 2, 3, to points in the body, then
268
a% [s1¢¢ s¢¢2 s¢¢3 ] = 0
and since [ s1¢¢ s¢¢2 s¢¢3 ] is nonsingular, a% = 0 and a = 0. Thus, J¢¢ is positive definite. This is the
case for any homogeneous body whose volume is not zero.
The 3 ´ 3 positive semidefinite matrix J¢¢ has three orthonormal eigenvectors (Kreyszig,
2011), denoted f ¢¢, g¢¢, and h¢¢ in the x ¢¢-y¢¢-z¢¢ frame, with corresponding nonnegative eigenvalues,
z i , i = 1, 2, 3, ordered such that z1 ³ z 2 ³ z 3 , where
J¢¢f ¢¢ = z1f ¢¢
J¢¢g¢¢ = z 2 g¢¢ (4.4.18)
J¢¢h¢¢ = z 3h¢¢
Since -g¢¢ and -h¢¢ are also eigenvectors, their signs can be arranged so that f ¢¢, g¢¢, and h¢¢ are
unit coordinate vectors for a right-hand x ¢-y¢-z¢ Cartesian frame. The transformation matrix
CT from the x ¢-y¢-z¢ frame to the x ¢¢-y¢¢-z¢¢ frame is, by Eq. (2.4.16),
CT = [f ¢¢ g¢¢ h¢¢] (4.4.19)
Substituting CT from Eq. (4.4.19) into Eq. (4.4.16),
J¢ = CJ ¢¢CT = [ f ¢¢ g¢¢ h¢¢] J¢¢ [ f ¢¢ g¢¢ h¢¢]

T
= [f ¢¢ g¢¢ h¢¢] [ J ¢¢f ¢¢ J¢¢g¢¢ J¢¢h¢¢]

T
= [f ¢¢ g¢¢ h¢¢] [ z1f ¢¢ z 2 g¢¢ z 3h¢¢]

T
(4.4.20)
é z1 0 0ù
= [f ¢¢ g¢¢ h¢¢] [f ¢¢ g¢¢ h¢¢] êê 0 z 2 0 úú
T
êë 0 0 z 3 úû
éz 1 0 0 ù
= êê 0 z 2 0 úú
êë 0 0 z 3 úû
Thus, in the x ¢-y¢-z¢ frame, J x ¢x ¢ = ζ1 , J y¢y¢ = ζ 2 , J z¢z¢ = ζ3 , and J x¢y ¢ = J x¢z¢ = J y ¢z¢ = 0 . The axes of such
a centroidal frame are called principal axes and the associated moments of inertia are called
principal moments of inertia. By construction, the principal products of inertia are zero.
4.4.4 Transformation of Planar Polar Moment of Inertia
As a special case, consider the body shown in Fig. 4.4.1 to be planar. The location of the
centroid relative to the x ¢¢-y¢¢ frame is ρ¢¢ = (1 / m ) ò s¢¢ dm(P) , but the polar moment of inertia is
P
the simpler form of Eq. (4.2.14), J ¢¢ = ò s¢¢ s ¢¢ dm(P) . To relate polar moments of inertia in the
P P
T
269
noncentroidal x ¢¢-y¢¢ and centroidal x ¢-y¢ frames, substitute s¢¢ = ρ¢¢ + s¢ from Fig. 4.4.1 into the
P P
integral for ρ¢¢ ,
( ) (ρ¢¢ + s¢ ) dm(P)
T
J ¢¢ = ò ρ¢¢ + s¢
P P
= ρ¢¢ ρ¢¢ò dm(P) + 2ρ¢¢ò s¢ dm(P) + ò s¢ Ts¢ dm(P)

P P P
T
(4.4.21)
m m m
= J ¢ + m ρ¢¢
2
This is a much simpler transformation than in Eq. (4.4.10).

4.4.5 Inertia Properties of Complex Bodies
Components of machines are often made up of subcomponents that have standard shapes;
e.g., circles, disks, cylinders, spheres, and rectangular solids. A typical example of such a
component is shown in Fig. 4.4.3, in which each subcomponent and void are of some standard
shape that is, typical of those resulting from standard manufacturing processes. The objective
now is to develop expressions for the inertia properties of the composite body, using easily
calculated properties of individual subcomponents.
Figure 4.4.3 Composite Body Made up of Subcomponents

Let an x ¢¢-y¢¢-z¢¢ frame be fixed in the composite body at a convenient location and
orientation; e.g., as shown in Fig. 4.4.3. In this frame, the centroid of the composite body may be
270
obtained using the definition of Eq. (4.4.2), employing the property that an integral over the
entire mass may be written as the sum of integrals over subcomponents mi , to obtain
1 k 1 k
ρ¢¢ = å
m i =1 mòi
¢¢ = å miρ¢¢i
P
s dm(P) (4.4.22)
m i =1
k
where m = å mi . To use this result, the centroid ρ¢¢i is first located in the x ¢¢-y¢¢-z¢¢ frame, using
i=1
mass and centroid location information from Table 4.4.1 or from direct numerical calculation.
Equation (4.4.22) is then used to locate the centroid of the composite body in the x ¢¢-y¢¢-z¢¢ frame.
The inertia matrix J* with respect to the x*-y*-z* centroidal reference frame of the
composite body; e.g., as shown in Fig. 4.4.3, is now to be calculated. The notation x*-y*-z* is
selected here to avoid confusion with the centroidal x ¢i -y¢i -z¢i frame of subcomponent i. Equation
(4.4.20) may be written and its integral evaluated as a sum of integrals over subcomponents that
make up the composite body, to obtain
k æ ö k
J* = - ò s% * s% * dm(P) = å ç - ò s% * s% * dm(P) ÷ = å J *i
P P P P
(4.4.23)
ç
i=1 è m i
÷ i=1
m ø
To use Eq. (4.4.23), the inertia matrix J*i of subcomponent i in the x*-y*-z* frame must be
calculated. To do this, a convenient x ¢i -y¢i -z¢i centroidal frame is defined for subcomponent i in the
composite body. Denote by J¢i the inertia matrix of subcomponent i with respect to its selected
centroidal x ¢-y¢-z¢ frame and by ρ¢i the vector that locates the centroid of the subcomponent in the
x*-y*-z* frame. Since the x*-y*-z* frame is not centroidal for the individual subcomponents, the
inertia transformation of Eq. (4.4.10) is used to calculate the inertia matrix of subcomponent i
with respect to the composite body centroidal x*-y*-z* frame. Denoting by C¢i the
transformation matrix from the x*-y*-z* frame to the x ¢i -y¢i -z¢i frame, Eq. (4.4.10) yields
(
J*i = C¢i T J¢iC¢i + mi ρ¢i Tρ¢i I - ρ¢ρ¢T ) (4.4.24)
This result may be substituted into Eq. (4.4.23), to obtain the desired inertia matrix for the
composite body. If there are voids in a composite body, Eq. (4.4.23) may be used by
assigning - J *i as the inertia matrix of the void, where J*i is the inertia matrix of a body that
would occupy the void, with the same material density as the body from which it is removed.
Inertia properties of a few homogeneous bodies, for use in such calculations, are provided in
Table 4.4.1.
_______________________________________________________________________
Example 4.4.1: The body of Fig. 4.4.4 is formed by a rectangular solid with a hole of radius R =
c/4 about the x axis. Since all three coordinate planes are planes of symmetry, the centroid is at
the origin of the x-y-z frame and the products of inertia with respect to this frame are zero.
271
Figure 4.4.4 Rectangular Solid With Hole
From Eq. (4.4.24), with J1 as the inertia matrix of the rectangular solid without a hole and J2 as
the inertia matrix of a cylinder that would occupy the hole, J = J1 - J 2 . Using Table 4.4.1,
γabc 2 2 πγc 4 a
J xx =
12
( b +c ) - 512
γabc 2 2 πγc 2 a æ 3c 2 2 ö
J yy = ( a +c ) - 192 ç 16 +a ÷
12 è ø
γabc 2 2 πγc 2 a æ 3c 2 2 ö
J zz = ( a +b ) - 192 ç 16 +a ÷
12 è ø
_______________________________________________________________________
272
Table 4.4.1 Inertia Properties of Some Homogeneous Bodies
273
The relatively elementary body of Example 4.4.1 shows that the transformation relations
for inertial properties and the theory of calculating centroids and inertia properties of complex
bodies can be used systematically. The reader may, however, despair over the extent of such
computations for complex composite bodies, such as automobile bodies, aircraft landing gear
assemblies, vending machine components, and robot arms. Indeed, manual application of the
methods used in this section would lead to extensive algebraic manipulations that are time
consuming and prone to error when carried out analytically by the engineer.
The availability of large-scale computer codes for system dynamic simulation creates
new opportunities for complex applications, but requires detailed inertia data that have not
normally been calculated in engineering practice. This dilemma may be alleviated using
computer-aided design and computer-aided engineering systems that contain extensive modeling
software that describes the geometry of complex mechanical assemblies. The methods developed
and illustrated in this section form the foundation for computer implementation with geometric
modelers to determine the locations of centroids and moments and products of inertia with
respect to reference frames that are specified by the engineer. The technical challenge in
implementing such methods is computer generation of the geometric definition of complex
bodies, which is the domain of solid modeling methods that are emerging in the form of practical
computational tools in modem computer-aided design and computer-aided engineering systems.
274
Once geometric and material property information is defined and entered into a computer
database, implementation of the analytical methods presented in this section for calculating the
locations of centroids and moments and products of inertia is a relatively simple matter. The
viewpoint taken for the remainder of this text is that such modern computer-aided engineering
tools are available to the engineer for computation of the inertial properties of machine
components.
The integral representation of the inertia matrix and location of centroid enable derivation of
formulas for transformation of these properties to body-fixed coordinate systems that have
attractive properties; e.g., a diagonal inertia matrix. Symmetry of bodies with respect of planes
and axes likewise yields simplified expressions. Finally, transformation formulas can be used
with composite bodies that are comprised of subsets with common shapes to evaluate inertia
properties, as is done in modern computer-aided engineering systems.
275
4.5 Internal Generalized Forces
Externally applied forces are accounted for in Eqs. (4.2.15) and (4.2.16) for planar
systems and Eqs. (4.3.12) and (4.3.13) for spatial systems. Internal forces due to force elements
that act between points fixed on pairs of bodies and exert forces of action and reaction along the
line between the attachment points are treated in this section. Such elements include passive
devices such as springs and dampers that act on pairs of bodies. Perhaps more important in
modern systems are actuators associated with control systems that exert forces and torques
between bodies using electric and hydraulic actuators. Internal forces associated with a
translational spring-damper-actuator (TSDA) that acts between points on bodies and a
rotational spring-damper-actuator (RSDA) that acts about common rotational axes in the bodies
are treated in this section, for both planar and spatial systems.
4.5.1 Internal Forces in Spatial Systems
Consider the TSDA shown in Fig. 4.5.l that connects points Pi and Pj on bodies i and j,
respectively.
Figure 4.5.1 Translational Spring-Damper-Actuator (TSDA)

The magnitude of the internal force that acts in the TSDA, with tension (drawing Pi and
Pj together) taken as positive, is
0 ( )
f = K ( l - l ) + Cl& + F l, l& (4.5.1)
where K is a spring constant, l 0 is the free length of the spring, C is a damping coefficient, and
( )
F l, l& is a general actuator force. Since an increase in l acts in the opposite sense as the force
f, the virtual work done on the system is negative if both f and δl are positive, so
δW = - fδl (4.5.2)
To see that the minus sign is required, with the augmentation of Fig. 4.5.2, note that if
δl > 0, f > 0, and as the attachment points on the bodies move apart, work done on the bodies is
negative and δW = -fδl .
276
Figure 4.5.2 Translational Spring-Damper-Actuator (TSDA)
The goal is to expand the right side of Eq. (4.5.2) in terms of variations in generalized
coordinates, to obtain
dW = dqTi Qi + dqTj Q j (4.5.3)
where Qi and Q j are vectors of generalized force that act on bodies i and j. This is a generally
applicable approach to determining generalized forces on bodies, using the virtual work of
applied forces.
The vector from Pi to Pj is
dij = rj + A js¢j - ri - A is¢i (4.5.4)
Thus, the length l of the spring-damper-actuator is given by

l 2 = dTij dij ³ 0 (4.5.5)
It is assumed that the TSDA is designed so that l > 0 . Differentiating Eq. (4.5.5) with respect to
time yields
2ll& = 2dTij d& ij (4.5.6)
so, using Eq. (2.6.26),
ij ij ij j j (
&l = (1/ l)d Td& = (1/ l)d T r& + B(p , s¢ )p& - r& - B(p , s¢ )p&
j j i i i i ) (4.5.7)
where it is important that the design requires that l > 0 . Similarly,

Taking the variation of Eq. (4.5.5), using Eq. (2.4.67),
δl = (1 / l)d Tij ( δrj + B(p j , s¢j )δp j - δri - B(p i , s¢i )δpi ) (4.5.8)
Substituting this result into Eq. (4.5.2) yields

δW = - ( f/l ) d Tij ( δrj + B(p j , s¢j )δp j - δri - B(pi , s¢i )δp i )
æ é d ij ùö æ é d ij ù ö (4.5.9)
= ëé δrjT δp Tj ûù çç - ( f/l ) ê T ú ÷÷ + ëéδri
T
δpiT ûù çç ( f/l ) ê T ú ÷÷
è ë B ( p j , s¢j )d ij û ø è ë B (pi , s¢i )dij û ø
277
The Euler parameter form of TSDA generalized forces on bodies i and j are thus
é d ij ù
Q i = ( f/l ) ê T ú
ë B (pi , s¢i )dij û
(4.5.10)
é d ij ù
Q j = - ( f/l ) ê T ú
ë B (p j , s¢j )d ij û
Another useful internal force element in spatial systems is the rotational spring-damper-
actuator (RSDA) that acts about the common axis of relative rotation in a revolute or cylindrical
joint between bodies i and j at common points Pi and Pj shown schematically in Fig. 4.5.3. A
torque
t = K( qi j - q0i j ) + Cq& ij + T(t) (4.5.11)
acts between axes fixed in bodies i and j, about the common z¢i and z¢j axes, with a positive
torque drawing the axes together. The angle q0i j in the RSDA corresponds to the free length l 0
of a TSDA. The virtual work due to the torque is thus
dW = -tdqi j (4.5.12)
Figure 4.5.3 Spatial Rotational Spring-Damper-Actuator (RSDA)

The angle qi j (p i , p j ) is given by Eq. (2.4.37),
ì-p - Arcsin( s ), if s £ 0 and c < 0

ï
qi j = í Arcsin(s ), if c ³ 0 (4.5.13)
ï p - Arcsin(s ), if s ³ 0 and c<0
î
where c º cosqi j = v¢xi T A Ti A j v¢xj and s º sinqi j = v¢yi T A iT A j v¢xj , and may be numerically evaluated
using MATLAB Code 2.4 in Section 2.B.2 of Appendix 2.B. In terms of angular velocities of
bodies i and j and the vector v¢zi ,
θ& ij = v iz T ( w j - wi ) = v¢zi T A iT (A jw¢j - A i w¢i ) = v¢zi T (A iT A jw¢j - w¢i )

(4.5.14)
= 2v¢ziT (A Ti A jG(p j )p& j - G(pi )p& i )
278
Similarly,
δθij = 2 v¢zi T (A iT A jG(p j )δp j - G( pi )δp i ) (4.5.15)
The torque of Eq. (4.5.11) that acts between the bodies is thus
t = K(qi j (p i , p j ) - θ0i j ) + C2 v¢ziT (A Ti A jG(p j )p& j - G(p i )p& i ) + T(t) (4.5.16)
and the virtual work of Eq. (4.5.12) may be written as

δW = -2tv¢ziT (A iT A jG(p j )δp j - G(pi )δp i )
(4.5.17)
= 2tδp Ti G T (pi ) v¢zi - 2tδp Tj GT (p j )A Tj A i v¢zi
Generalized forces that act on bodies i and j, as a result of the RSDA, are thus
é 0 ù
Qi = ê iú
ë 2tG (pi ) v¢z û
T
(4.5.18)
é 0 ù
Qj = ê iú
ë -2tG (p j )A j A i v¢z û
T T
The effect of TSDA and RSDA generalized forces on the equations of motion of a system
is accounted for by inserting the expressions of Eqs. (4.5.10) and (4.5.18) into the variational
equations of motion.
4.5.2 Internal Forces in Planar Systems
The derivation of TSDA generalized forces for planar systems is the same as that for
spatial systems, through Eq. (4.5.7). Taking the derivative and variation of l in Eq. (4.5.5) yields
(
&l = (1/ l ) d T r& + f& PA s¢ - r& - f& PA s¢
ij j j j j i i i i )
(4.5.19)
δl = (1/ l ) dijT ( δrj + δf jPA js¢j - δri - δfi PAi s¢i )
Substituting δl into Eq. (4.5.2) yields

δW = - ( f/l ) dijT ( δrj + δf jPA js¢j - δri - δfi PA is¢i )
æ é dij ù ö æ é dij ù ö (4.5.20)
= éë δrjT δf j ùû ç - ( f/l ) ê T ú ÷÷ + éë δri
T
δfi ùû ç ( f/l ) ê T ú ÷÷
ç ç
è ëd ij PA js¢j û ø è ëdij PA is¢j û ø
Thus, TSDA generalized forces that act on bodies i and j are
é dij ù
Qi = ( f/l ) ê T Pú
ëdij PAi s¢i û
(4.5.21)
é dij ù
Q j = - ( f/l ) ê T ú
ëdij PA js¢j û
279
Another useful internal force element in planar systems is the rotational spring-damper-
actuator (RSDA) that acts about the axis of relative rotation in a revolute joint between bodies i
and j at common points Pi and Pj shown schematically in Fig. 4.5.2. A torque
t = K(f j - fi - f0 ) + C(f& j - f& i ) + T (4.5.22)
acts between axes fixed in bodies i and j, parallel to the x ¢i and x¢j axes, with a positive torque
drawing the axes together. The angle f0 in the RSDA corresponds to the free length l 0 of a
TSDA. The virtual work due to the torque is thus
dW = -t(df j - dfi ) = -tdf j + tdfi (4.5.23)
The generalized forces acting on bodies i and j are thus

é0 ù
Qi = ê ú
ëtû
(4.5.24)
é0ù
Qj = - ê ú
ëtû
Figure 4.5.2 Planar Rotational Spring-Damper-Actuator (RSDA)
Generalized forces due to internal force elements such as springs, dampers, and actuators that act
between pairs of bodies are accounted for using virtual displacements and rotations to calculate
the associated virtual work .
280
4.6 Variational Equations of Motion for Multibody Systems
Mechanical systems that are made up of multiple particles, planar bodies, and spatial
bodies are considered. Holonomic and nonholonomic kinematic constraints are accounted for.
The variational equation of motion for such systems is formed by summing variational equations
for each of its particles and bodies. Constraints that act between particles and bodies in the
system are accounted for by requiring that virtual displacements satisfy the linearized form of
constraint equations, with time fixed.
4.6.1 System Constraints and Virtual Displacements
For systems that are comprised of particles, planar bodies, and spatial bodies, holonomic
constraints between bodies i and j are of the form
F k (qi , q j ) = 0 (4.6.1)
T
k = 1L nh , where generalized coordinates are q = éëq1 qT2 L qTnb ùû and Euler parameter
T
normalization conditions for spatial bodies are included in Eq. (4.6.1). The associated kinematic
velocity equations are obtained by taking the time derivative of Eq. (4.6.1),
F qki&q i + F qk j&q j = 0 (4.6.2)
and virtual displacement equations are obtained by taking the differential of Eq. (4.6.1), with
time held fixed,
Fqki δqi + Fqkj δq j = 0 (4.6.3)
Velocity constraints, possibly nonholonomic, between bodies i and j are

Eik (q i , q j ,t)q& i + Ekj (qi , q j , t)q& j = e k (qi , q j , t) (4.6.4)
k = nh + 1L nh + nd = nc . The associated virtual displacement equations are

Eik δqi + Ekj δq j = 0 (4.6.5)
As in Section 3.4, define the Jacobian for constraint k of Eq. (4.6.1) or (4.6.4) relative to
body i as
C ºí
k
i , i = 1L nb (4.6.6)
ïî0 , otherwise
The full Jacobian for constraint k that includes contributions from all bodies is
T
C k º éëC1k T C2kT L C knb T ùû
where, as defined in Eq. (4.6.6), only Cαk and Cβk are nonzero for bodies α and β that are
connected by constraint k. For holonomic constraint k, this is Cik = F qki and Ck = F qk . The system
constraint Jacobian, or system Jacobian, is
281
T
C º éëC1T C2T L CncT ùû (4.6.7)
For systems that involve many bodies and constraints, the system Jacobian is highly sparse; i.e.,
its entries are nearly all zeros. Finally, the system velocity constraints of Eqs. (4.6.2) and (4.6.4)
are
é 0 ù
Cq& = ê ú (4.6.8)
ëe(q i , q j , t )û
And the system virtual displacement conditions are
Cδ q = 0 (4.6.9)
4.6.2 Variational Equations of Motion
For a particle with generalized coordinate qi = ri or a planar body with centroidal
T
generalized coordinate qi = éëriT fi ùû , its variational equation of motion, from Eqs. (4.2.20), is
(
dq Ti Mi &&
qi - QiA = 0 ) (4.6.10)
where for a particle,

M i = mi I 3
(4.6.11)
QiA = Fi
and for a centroidal body reference frame,
ém I 0ù
Mi = ê i 3
ë 0 J ¢i úû
(4.6.12)
é FA ù
Q = ê iA ú
A
ë n ¢i û
i
If noncentroidal body reference frames are used for planar bodies, kinetic terms of Eqs. (4.2.19)
must be used.
T
For a spatial body with generalized coordinates qi = éëriT piT ùû , the variational equation
of motion of Eq. (4.3.23) is
dq Ti ( M i q
&& i - Si - QiA ) = 0 (4.6.13)
where, for a centroidal body reference frame,
282
é mi I 3 0 ù
Mi = ê
ë 0 4G (pi )J ¢iG (pi ) úû
T
é 0 ù
Si = ê T ú (4.6.14)
ë8G (p& i )J ¢i G (p i )pi û
é FA ù
QiA º ê T i Aú
ë 2G (pi )n¢i û
If the body reference frame is noncentroidal, kinetic terms of Eqs. (4.3.26) must be used.
With Si = 0 for particles and summing the variational equations of Eqs. (4.6.10) and
(4.6.13) over all bodies, the system variational equation of motion is
å dq ( M q&& - Si - Q iA ) = 0
nb
T
i i i (4.6.15)
i =1
which must hold for all dq i that satisfy Eqs. (4.6.3) and (4.6.5). Forces of constraint in the
system do not appear in Eq. (4.6.15), because they do no work under the action of kinematically
admissible virtual displacements, dq .
Defining
T
q = éëq1T L q Tnb ùû
T
dq = éë dq1T L dq nb
T
ùû
M = diag ( M1 L M nb ) (4.6.16)
T
Q A = éëQ A1T L Q ATnb ùû
T
S = éëS1T L S Tnb ùû
Eq. (4.6.15) may be written as

δq T ( M (q)&&
q - S(q, q& ) - QA (q, q& , t) ) = 0 (4.6.17)
With the system constraint Jacobian C of Eq. (4.6.7), the system virtual displacement dq of Eq.
(4.6.16) must satisfy the differential form of the velocity constraint of Eq. (4.6.8); i.e.,
Cdq = 0 (4.6.18)
The system variational equation of motion is thus that Eq. (4.6.17) holds for all dq that satisfy
Eq. (4.6.18); i.e., for all kinematically admissible virtual displacements.
4.6.3 Positive Definiteness of System Mass Matrix
For the single spatial body shown in Fig. 4.3.1, the velocity of a differential element of
mass at point P is
r& P = r& + w % ¢s¢P = r& - As% ¢P w¢
% s P = r& + Aw (4.6.19)
283
With this relation, the kinetic energy of body i is
1
( r& - As%¢P w¢) ( r& - As%¢Pw¢) dm(P)
T
KEBi =
2mò
m T 1
= r& r& - ò w¢Ts% ¢P A T As% ¢P w¢dm(P) - ò r& T As%¢Pw¢dm(P)
2 2m m (4.6.20)
m 1
= r& Tr& + w¢T J ¢w¢ - mr& T As% ¢c w¢
2 2
m 1
= r& Tr& + 2p& TGT J ¢Gp& - 2mr& T As%¢cGp& = q& iT M i q& i
2 2
where Eq. (2.6.63) is used to write the equation in terms of Euler parameter time derivatives and
the definition of the noncentroidal mass matrix is given in Eq. (4.3.26). Since the integrand on
the right of Eq. (4.6.20) is nonnegative at all points in the body, KEB i ³ 0 for all r& and w ,
hence all q& i . Thus, the body mass matrix M i is positive semidefinite.
To see that M i for a spatial body in the Euler parameter form of the equations of motion
is not positive definite, let p& = p ¹ 0 and r& = 0 ; i.e., q& = éë0 p ùû ¹ 0 . The identity of Eq.
T T
(2.6.6), which holds even if p does not satisfy the normalization condition, shows that
Gp& º G(p)p& = G (p)p = 0 . For this q& , the last equality of Eq. (4.6.20) shows that q& T M q& = 0 .
i
Thus, M i is not positive definite. In fact, it is singular. Note, however, that the generalized
velocity p& = p chosen satisfies p T&p = 1 if p satisfies the Euler parameter normalization
constraint. Thus, the velocity &p is not kinematically admissible.
Summing Eq. (4.6.20) over all bodies in the system, using the notation of Eq. (4.6.16),
yields the kinetic energy for the entire system as
1 1
KES = å
2 i
q& i T M i q& i = q& T Mq&
2
(4.6.21)
Since each body mass matrix M i is positive semidefinite, the system mass matrix M is positive
semidefinite.
Kinematically admissible virtual velocities are those that satisfy the holonomic constraint
velocity equations with time held fixed; i.e.,
F q dq& = 0
and homogeneous differential constraints,

Edq& = 0
Combined, this is
éF ù
Cq& º ê q ú dq& = 0 (4.6.22)
ëEû
284
Where, for a proper design and model, C has full rank.
The kinetic energy of virtual velocities is (1 / 2 ) dq& T Mdq& . Physical reasoning leads to the
conclusion that the only kinematically admissible virtual velocity for which system kinetic
energy is zero is the zero velocity; i.e., dq& = 0 . Otherwise, there exists a motion with nonzero
virtual velocity that is realized without having done any work on the system to achieve that
motion from a configuration at rest. If the reader can find such a system in the real world, fame
and fortune will surely follow. No such real system exists. Thus, real system kinetic energy is
positive definite on the space of kinematically admissible virtual velocities; i.e., the null space of
the constraint Jacobian. As shown above, this does not say that matrix M is positive definite.
The foregoing result does not say that the system mass matrix of a “physically bad
model” is positive definite on the null space of the model constraint Jacobian. If a model is
created using one or more bodies of finite dimension but zero mass and inertia matrix, there is no
guarantee that the resulting mass matrix is positive definite on said null space. Similarly, if an
Euler parameter normalization condition is not included in the vector of holonomic constraints,
the definiteness result is invalidated. Simply stated, the foregoing mathematical result depends
on kinematic modeling and kinetic modeling that represent the actual mechanics of the system
considered.
The results of this section are valid for planar systems and systems of particles, as may be
verified by even simpler manipulations, using the equations of Section 4.6.1.
4.6.4 Initial Conditions
In addition to the equations of motion of Eqs. (4.6.17) and (4.6.18), initial conditions
must be specified for q 0 and q& 0 at an initial time t 0 , consistent with constraints of Eqs. (4.6.1),
(4.6.2), and (4.6.4). The number of initial position and orientation conditions should be equal to
the number of degrees of freedom; i.e., the number of generalized coordinates ngc minus the
number nhc of independent holonomic constraint equations of Eq. (4.6.1), written in the form
é F1 (q0 ) ù
ê ú
F (q0 ) = ê M ú=0 (4.6.23)
êëF (q )úû
nhc 0
A general form of ngc - nhc specified initial configuration conditions is

F I ( q0 , t 0 ) = 0 (4.6.24)
These conditions must be independent among themselves, as well as with the kinematic
constraints of Eqs. (4.6.23) at t 0 . The combined conditions,
é F( q 0 ) ù
ê I 0 0 ú=0 (4.6.25)
êëF ( q , t )úû
must uniquely determine q0 = q(t 0 ) , which requires the Jacobian of Eq. (4.6.25) with respect to
q0 is nonsingular. Equations (4.6.25) may be solved using iterative numerical methods for q 0 .
285
Similarly, initial velocity conditions may be specified at t 0 as
BI (q0 )q& 0 = n I (4.6.26)
where the (ngc - nhc - nd) ´ ngc matrix B I (q0 ) must be of full rank and independent of the
velocity constraint conditions of Eq. (4.6.6) at t 0 , written in the form
éF q ( q 0 ) ù 0 é 0 ù
C(q )q = ê
& 0
0
0 ú
q& = ê 0 ú º n 2 (4.6.27)
ë E(q ) û ëe(q )û
where C(q 0 ) is nonsingular. The system of ngc linear equations of Eqs. (4.6.26) and (4.6.27)
may thus be solved for initial velocity conditions q& 0 .
If initial conditions for spatial bodies are specified in terms of angular velocity at t 0 ; i.e.,
B Ir& 0 + CIp& 0 + DI w¢0 = n I
they may be transformed to the form of Eq. (4.6.26) by using the relation w¢ = 2G (p 0 )p& of Eq.
(2.6.63); i.e.,
BIr& 0 + CIp& 0 + DIw¢0 = BIr& 0 + CIp& 0 + 2DIG (p0 )p& 0 = n I
Initial conditions for planar systems may be defined in the same way.
Variational equations of motion for a multibody system are obtained by summing variational
equations for each body in the system. The result is a single variational equation of motion that
must hold for all generalized coordinate variations (virtual displacements) that are consistent
with holonomic and nonholonomic constraints that act on the system. Due to d’Alembert’s
principle, this variational equation need not include constraint reaction forces.
Under the physically meaningful condition that kinetic energy is positive for any nonzero
kinematically admissible virtual velocity, the system mass matrix is positive definite on the
space of kinematically admissible virtual velocities. This is a critically important property in
implementing any numerical solution method to simulate the dynamics of multibody systems.
Key Formulas
F k (qi , q j , t) = 0 Eik (qi , q j , t)q& i + Ekj (qi , q j , t)q& j = e k (qi , q j , t) (4.6.1) (4.6.4)

C ºí
k
i , i = 1L nb (4.6.6)
ïî0 , otherwise
M = diag ( M1 L M nb )
T T
q = éëq1T L q Tnb ùû dq = éë dq1T L dq Tnb ùû
T T
(4.6.16)
Q A = éëQ AT1 L Q A Tnb ùû S = éëS1T L S Tnb ùû
δq T ( M (q)&&
q - S(q, q& ) - QA (q, q& , t) ) = 0 Cdq = 0 (4.6.17) (4.6.18)
286
4.7 ODE of Motion in Independent Generalized Coordinates
For holonomic multibody systems whose kinematics can be defined using independent
generalized coordinates, the variational equations of motion derived in Section 4.6 can be
reduced to ordinary differential equations (ODE). To illustrate the process, examples that
involve particles, planar bodies, and spatial bodies are analyzed in Section 4.7.1. A general
formulation is presented in Section 4.7.2, and results on existence, uniqueness, and
differentiability of solutions of ODE are summarized in Section 4.7.3. An extension for
nonholonomic systems is presented in Chapter 6.
4.7.1 Examples of Systems with Independent Generalized Coordinates
_______________________________________________________________________
Example 4.7.1 The planar double pendulum model of Fig. 4.7.1 is comprised of two particles
with nonzero mass that are connected by massless bars of unit length. Bar one is pivoted at the
origin of the x-y frame and bar two is pivoted relative to bar one at mass m1 . The angles of
rotation v1 and v2 serve as generalized coordinates that are subject to no constraint; i.e., they are
independent generalized coordinates. Gravitational force acts in the negative y-direction.

Vectors that locate the masses in the inertial x-y frame are
r1 = [ cos(v1 ) sin(v1 ) ]
T
(4.7.1)
r2 = [ cos(v1 ) sin(v1 ) ] + [cos(v 2 ) sin(v 2 ) ]
T T
Their first derivatives and variations yield velocities and virtual displacements of the masses as
&r1 = [ -sin(v1 ) cos(v1 )] &v1
T
&r2 = [ -sin(v1 ) cos(v1 ) ] &v1 + [ -sin(v 2 ) cos(v 2 ) ] &v 2

T T
(4.7.2)
dr1 = [ -sin(v1 ) cos(v1 )] dv1
T
dr2 = [ -sin(v1 ) cos(v1 ) ] dv1 + [ -sin(v 2 ) cos(v 2 )] dv 2

T T
Finally, the derivative of velocities yields accelerations of the masses,
287
&&r1 = [ -sin(v1 ) cos(v1 )] &&v1 - [cos(v1 ) sin(v1 )] (v
& 1 )2
T T
&&r2 = [ -sin(v1 ) cos(v1 ) ] &&v1 - [cos(v1 ) sin(v1 )] (v

& 1 )2
T T
(4.7.3)
+ [ -sin(v 2 ) cos(v 2 )] &&v 2 - [cos(v 2 ) sin(v 2 )] (v
& 2 )2
T T
The variational equation of motion of the system of two particles, from Eq. (4.1.25), is
dr1T ( m&&
1r1 + m1gu y ) + dr2 ( m 2 r2 + m 2 gu y ) = 0
T
&& (4.7.4)
where u y is the unit vector along the positive y axis and g is gravitational acceleration.
Substituting from Eqs. (4.7.2) and (4.7.3) and collecting terms, Eq. (4.7.4) reduces to
&& 1 + m 2 ( sin(v1 )sin(v 2 ) + cos(v1 )cos(v 2 ) )&&v 2
æ (m1 + m 2 )v ö
dv1 ç ÷ =0
ç + m ( sin(v )cos(v ) - cos(v )sin(v ) ) (v & ) 2
+ (m + m )gcos(v ) ÷
è 2 1 2 1 2 2 1 2 1 ø
(4.7.5)
æ m 2 ( sin(v1 )sin(v 2 ) + cos(v1 )cos(v 2 ) )&&v1 + m 2&&v 2 ö
dv 2 ç ÷ =0
ç + m ( cos(v )sin(v ) - sin(v )cos(v ) ) (v & ) 2
+ m gcos(v ) ÷
è 2 1 2 1 2 1 2 2 ø
Since v1 and v2 are independent; i.e., subject to no constraints, dv1 and dv2 are arbitrary.
Thus, their coefficients in Eq. (4.7.5) must be zero. Using the trigonometric identities
sin(v1 )sin(v2 ) + cos(v1 )cos(v 2 ) = cos(v2 - v1 ) and cos(v1 )sin(v2 ) - sin(v1 )cos(v 2 ) = sin(v2 - v1 ) in
Eq. (4.7.5) yields the second order system of ODE
&& 1 + m2 cos(v2 - v1 )v
(m1 + m2 )v && 2 - m2sin(v2 - v1 )(v
& 2 )2 = -(m1 + m2 )gcos(v1 )
(4.7.6)
&& 1 + m2&&v2 + m2sin(v2 - v1 )(v
m2cos(v2 - v1 )v & 1 ) 2 = -m2gcos(v2 )
In matrix form, Eqs. (4.7.6) are
é (m1 + m2 ) m2cos(v2 - v1 ) ù é&&v1 ù é-(m1 + m2 )gcos(v1 ) ù é -m2sin(v2 - v1 )(v
& 2 )2 ù
ê m cos(v - v ) ú ê&&v ú = ê -m gcos(v ) ú - ê & 1)2 û
ú
ë 2 2 1 m2 ûë 2û ë 2 2 û ë m2sin(v2 - v1 )(v
(4.7.7)
The determinant of the symmetric acceleration coefficient matrix of Eq. (4.7.7) is
(
(m1 + m2 )m2 - (m2 )2 (cos(v2 - v1 ))2 = m1m2 + (m2 ) 2 1 - (cos(v2 - v11 ))2 > 0 )
so the matrix is nonsingular. In fact, since each of the principal subdeterminants is positive, it is
positive definite (Strang, 1980).
With initial conditions; e.g., with both masses at rest on the x-axis at time t 0 = 0 ,
v1 (0) = v2 (0) = 0
(4.7.8)
&v1 (0) = &v2 (0) = 0
the initial-value problem of Eqs. (4.7.7) and (4.7.8) has a unique solution (Teschl, 2012).
________________________________________________________________________
288
________________________________________________________________________
Example 4.7.2 The planar slider-crank mechanism model of Fig. 4.7.2 is comprised of a crank
that is modeled as a planar rigid body of radius R and polar moment of inertia J1 , relative to its
centroid at the origin of its x ¢-y¢ body reference frame, and a slider along the x-axis that is
modeled as a point mass m 2 . The crank rotates about the origin of the inertial x-y frame and is
connected to the slider by a massless connecting rod of length 2 units.
Figure 4.7.2 Planar Slider-Crank

Provided R < 2 , the angle of rotation v of the crank is an independent generalized
coordinate that uniquely determines the geometry of the mechanism. With u = Rsin(v) and
u 2 + w 2 = (Rsin(v))2 + w 2 = 4 , w = 4 - (Rsin(v))2 and
x 2 = Rcos(v) + w = Rcos(v) + 4 - (Rsin(v)) 2 (4.7.9)

Differentiation yields the velocity, virtual displacement, and acceleration of the slider,
æ R 2sin(v)cos(v) ö
&x 2 = ç - Rsin(v) - ÷&v
ç 4 - (Rsin(v)) 2 ÷ø
è
dx 2 = ç - Rsin(v) - ÷ dv
ç 4 - (Rsin(v)) 2 ÷
è ø
(4.7.10)
&&x 2 = ç -Rsin(v) - ÷&&v
ç 4 - (Rsin(v)) 2 ÷
è ø
æ ö
R 2 (cos(v) 2 - sin(v)2 ) R 4 (sin(v)cos(v))2 ÷ 2
+ ç -Rcos(v) - - &v
ç
( ) ÷
3/ 2
4 - (Rsin(v)) 2 4 - (Rsin(v)) 2
è ø
For the crank, the position, velocity, virtual displacement, and acceleration are zero and the
angular acceleration and virtual rotation are &&v and dv , respectively.
Summing the variational equations of motion of the planar crank from Eq. (4.2.20) and of
the point mass slider from Eq. (4.1.21), the system variational equation of motion is
289
dvJ1&&v + dx 2 m2&&x 2 = 0 (4.7.11)
Substituting from Eqs. (4.7.10) and collecting terms, Eq. (4.7.11) reduces to
dv ( M(v)v & )=0
&& - g(v,v) (4.7.12)
where
2
M(v) º J1 + m 2 ç Rsin(v) + ÷ >0
ç 4 - (Rsin(v))2 ÷
è ø
g(v,&v) º - m 2 ç Rsin(v) + ÷ (4.7.13)
ç 4 - (Rsin(v))2 ÷
è ø
æ ö
ç R 2 (cos(v) 2 - sin(v) 2 ) R 4 (sin(v)cos(v)) 2 ÷ 2
´ Rcos(v) + - &v
ç ( ) ÷
3/ 2
4 - (Rsin(v))2 4 - (Rsin(v)) 2
è ø
Since v is independent, dv is arbitrary and its coefficient in Eq. (4.7.12) must be zero. This
yields the second order ODE
&& = g(v,v)
M(v)v & (4.7.14)
Since the coefficient of &&v is never zero, the initial-value problem of Eq. (4.7.14) and initial
conditions such as v(0) = 0 and &v(0) = w0 has a unique solution, provided R < 2 (Teschl, 2012).
Since a closed form solution is out of the question, numerical solution methods are called for.
________________________________________________________________________
________________________________________________________________________
Example 4.7.3 A three degree of freedom robotic manipulator, modeled as a special case of that
shown in Fig. 1.2.4, is shown in Fig. 4.7.3. The base, body 1, rotates by angle v1 about the
global z axis. Body 2 rotates by angle v 2 about the body fixed x¢¢2 axis that is parallel to the x1¢¢
axis in body 1. Finally, body 3 translates by distance v3 along the common y¢¢2 and y¢¢3 axes. The
height of body 1 is 2m, with the centroid at the midpoint. Bodies 2 and 3 are 2m and 4m in
length, respectively, with centroids at their midpoints.
290
4.7.3 Three Degree of Freedom Robotic Manipulator
For body 1,
r1 = u z
(4.7.15)
r&1 = &&
r1 = 0
and its angular velocity in the global frame is ω1 = v& 1u z . In the body 1 reference frame,
ω1¢ = A1Tω1 = v& 1u z = [ u z 0 0] v& º W1v& (4.7.16)
where, with c1 º cos(v1 ) and s1 º sin(v1 ) ,
éc1 -s1 0 ù
A1 = êê s1 c1 0 úú (4.7.17)
êë 0 0 1 úû
Taking differential and derivative yields virtual rotation and angular acceleration
dπ1¢ = W1dv
(4.7.18)
& 1¢ = W1&&
ω v
For body 2, with c2 º cos(v2 ) and s2 º sin(v2 ) , and the transformation matrix from the
body 2 frame to the body 1 frame,
é1 0 0 ù
A12 = êê 0 c 2 -s 2 úú (4.7.19)
êë 0 s 2 c 2 úû
r2 = 2r1 + A1A12u y¢¢ (4.7.20)
Taking time derivatives and variation yields velocity, acceleration, and virtual displacement,
291
r&2 = v& 1A1¢ A12u y¢¢2 + v& 2 A1A12
¢ u y2¢¢ = éë A1¢A12u y2¢¢ ¢ u y2¢¢
A1A12 0 ùû v& º V2 v&
dr2 = V2dv (4.7.21)
v + v& 12 A1¢¢A12u y¢¢2 + 2v& 1v& 2 A1¢A12
r2 = V2&&
&& ¢ u y¢¢2 + v& 22 A1A12
¢¢ u y2¢¢ º V2&&
v + U2
where a prime denotes derivative of entries in the associated matrices with respect to their
argument v1 or v2 and a double prime denotes two derivatives.
The angular velocity of body 2, represented in the body 1 reference frame, is
¢
(ω 2 )1 = v& 2 A12 u x1¢¢ . Transforming to the global frame, the angular velocity of body 2 is
ω 2 = ω1 + A1A12 (ω 2 )1¢ = v& 1u z + v& 2 A1A12 u x1¢¢
Transforming this vector to the body 2 reference frame,

ω¢2 = A12
T
A1T ω 2 = v& 1A12
T
A1T u z + v& 2u x1¢¢ = éë A12
T
A1T u z u x1¢¢ 0ûù v& º W2 v& (4.7.22)
taking the variation and time derivative yield the virtual rotation and angular acceleration
dπ¢2 = W2 dv
(4.7.23)
& ¢2 = W2 &&
ω v + v& 12 A12
T
A1¢T u z + v& 1v& 2 A12
¢T A1T u z º W2 &&
v + X2
Body 3 is constrained to have the same orientation as body 2, so
ω¢3 = ω¢2 = W2 v&
dπ¢3 = dπ¢2 = W2 dv (4.7.24)
& ¢3 = ω
ω & ¢2 = W2 &&
v + X2
The position of the centroid of body 3 is r3 = 2r1 + (v 3 + 2) A1 A12u y¢¢2 . Taking derivatives and
variation yield
r&3 = v& 1 (v3 + 2) A1¢ A12u y¢¢2 + v& 2 (v3 + 2) A1A12
¢ u y2¢¢ + v& 3 A1A12 u y2¢¢
= éë (v3 + 2)A1¢ A12u y¢¢2 ¢ u y¢¢2
(v3 + 2) A1A12 A1A12u y2¢¢ ùû v& º V3 v&
dr3 = V3dv (4.7.25)
æ v& 12 (v3 + 2) A1¢¢A12u y¢¢2 + 2v& 1 v& 2 (v3 + 2)A1¢ A12 ¢ u y¢¢2 ö
v+ç 2
r3 = V3 &&
&& ÷
ç + v& 2 (v3 + 2)A1 A12 ¢¢ u + &
2v &
v A ¢ A u + +2v & &
v A A ¢ u ÷
è y ¢¢
2 1 3 1 12 y ¢¢
2 2 3 1 12 y 2 ø
¢¢
º V3 &&
v + U3
Using inertia matrices of the bodies,
J1¢ = J1I 3
(4.7.26)
J¢2 = J¢3 = diag(2 0 5 20 )
and summing variational equations of motion of Eq. (4.3.16), the system variational equation of
motion is
292
dπ1¢T ( J1¢ω % 1¢ J1¢ω1¢ ) + dr2T ( m 2&&r2 + m 2gu z )
& 1¢ + ω
(4.7.27)
+ 2dπ¢2T ( J ¢2ω % ¢2 J1¢2ω¢2 ) + dr3T ( m3&&r2 + m3gu z ) = 0
& ¢2 + ω
Substitution from Eqs. (4.7.16) through (4.7.26), the variational equation of motion in the
variable v is
δv T ( M (v )&&
v - g (v,v& ) ) = 0 (4.7.28)
where
M(v) = W1T J1¢ W1 + m 2 V2T V2 + 2W2T J ¢2 W2 + m3V3T V3
²v& )J¢ W v& + m V T ( U + gu ) + 2 WT J ¢ X ö
æ W1T (W (4.7.29)
g(v,v& ) = - ç ÷
1 1 1 2 2 2 z 2 2 2
ç +2 W ( W
T ²v& )J ¢ W v& + m V ( U + gu )
T ÷
è 2 2 2 2 3 3 3 z ø
Since δv is arbitrary, its coefficient in Eq. (4.7.28) must be zero, yielding the ODE
v = g(v,v& )
M (v)&& (4.7.30)
The system kinetic energy is
KE = (1/ 2) v& T ( m1V1T V1 + W1T J1¢ W1 + m 2V2T V2 + 2 W2T J ¢2 W2 + + m3V3T V3 ) v&
(4.7.31)
= (1/ 2) v& T M (v)v& ³ 0
so M (v) is positive semidefinite. In fact, since kinetic energy must be positive for any nonzero
velocity v& , M (v) must be positive definite, hence nonsingular. Thus, the initial-value problem of
Eq. (4.7.30) and initial conditions such as v (0) = &v(0) = 0 has a unique solution (Teschl, 2012).
Oppressive calculations and manipulations would be required to obtain an explicit
analytical form of equations of motion, for even this modest multibody system. Needless to say,
a closed form solution is not to be had, so numerical methods are required to obtain the solution.
For computation, terms in Eq. (4.7.29) can be computer generated and numerical methods of
solution may be applied. This strongly suggests that systematic computer methods are required to
construct equations of motion for realistic multibody systems, as well as to obtain their solution.
____________________________________________________________________________
4.7.2 General Form of ODE of Multibody Dynamics
Consider a multibody system with generalized coordinates qÎ R ngc that must satisfy nhc
independent holonomic constraints
F (q ) = 0 (4.7.32)
i.e., Φq (q) has full row rank. Time derivatives of Eq. (4.7.32) yield the velocity and acceleration
constraints
Φ q (q )q& = 0
( )
&& + Φq (q )q&ˆ q& º Φ q (q)q
Φ q (q )q
q
&& + γ (q, q& ) = 0
(4.7.33)
293
Define independent generalized coordinates v Î R m , m = ngc - nhc, that determine
dependent generalized coordinates through the transformation
q = y (v ) (4.7.34)
and its differential and derivatives
dq = y v dv
q& = y v v& (4.7.35)
q = y v &&
&& ( )
v + y v v&ˆ v&
v
If v , v& , and &&

v identically satisfy the holonomic constraints; i.e.,
F (y (v )) = 0
Φ q ( y (v ) )y v (v )v& = 0 (4.7.36)
( ) ( )
v + Φ q (q)yˆ v (v )v&ˆ y v (v )v& + Φ q ( y (v ))y f v (v )vˆ& v& = 0
Φ q ( y (v ) )y v (v )&&
q v
v , then y(v ) is said to be a parameterization of the regular constraint

for arbitrary v , v& , and &&
manifold of Section 3.5,
{
C% = q:F (q ) = 0, rankΦ q (q ) = nhc } (4.7.37)
In the language of mechanics (Pars, 1965), v and its derivatives are called Lagrangian
generalized coordinates.
Substituting from Eqs. (4.7.35) into the variational equation of motion of the multibody
system, Eq. (4.6.16),
dv ê
T
é y Tv (v )M (y(v ))y v (v )&& (
v + y Tv (v )M(y (v )) y v (v )v&ˆ v& ù
v ú =0
) (4.7.38)
ê - y T (v )S(y (v ), y (v )v& ) - y T (v )Q A (y(v ), y (v )v& ) ú
ë v v v v û
which must hold for arbitrary dv . This yields the second order ODE
y Tv (v )M(y (v ))y v (v )&& (
v + y Tv (v )M(y(v )) y v (v )vˆ& v&
v
) (4.7.39)
- y v (v )S(y (v ), y v (v )v& ) - y v (v )Q (y(v ), y v (v )v& ) = 0
T T A
Since y v (v ) is a kinematically admissible virtual velocity, the coefficient matrix

y Tv (v )M (y (v ))y v (v ) is positive definite, hence nonsingular. The initial-value problem of Eq.
(4.7.39) and initial conditions such as v (0) = v& (0) = 0 thus has a unique solution (Teschl, 2012).
The conditions of Eq. (4.7.36) on Lagrangian generalized coordinates are very severe.
For example, as shown in Section 3.6, Lagrangian generalized coordinates do not exist for the
unit sphere in R 3 . If such coordinates can be found; e.g., as in examples 4.7.1 through 4.7.3, or
using the tangent space parameterization of Section 3.5.2, ODE of dynamics can be obtained.
The benefit of using Lagrangian generalized coordinate thus becomes evident. The generalized
coordinates q, q& , and &&
q of Eqs. (4.7.34) and (4.7.35) identically satisfy the constraints of Eqs.
294
(4.7.32) and (4.7.33) and satisfy the variational equation of motion, Eq. (4.6.16). They are thus
the unique solution of the equations of mechanical system dynamics.
_____________________________________________________________________________
Example 4.7.4: For the planar double pendulum of Section 4.7.1,
q = éër1T r2T ùû = [cos(v1 ) sin(v1 ) cos(v1 ) + cos(v 2 ) sin(v1 ) + sin(v 2 )] º y(v )

T T
Terms required in the ODE of Eq. (4.7.39) are S = 0 and

é -sin(v1 ) 0 ù
ê cos(v ) 0 ú
y v (v ) = ê 1 ú
ê -sin(v1 ) -sin(v 2 )ú
ê ú
ë cos(v1 ) cos(v 2 ) û
( y (v)v&ˆ )
T
v v& = éë -cos(v1 )v& 12 -sin(v1 )v& 12 -cos(v1 )v& 12 - cos(v 2 )v& 22 -sin(v1 )v& 12 - sin(v 2 )v& 22 ùû
v
M = diag ( m1 m1 m2 m2 )
Q A = [0 - m1g 0 -m 2 g]
T
Substituting these relations into Eq. (4.7.39) and collecting terms yields
é (m1 + m2 ) m2 cos(v2 - v1 ) ù é&&v1 ù é-m2sin(v2 - v1 )(v
& 2 )2 ù é-(m1 + m2 )gcos(v1 ) ù
ê m cos(v - v ) +
ú ê&&v ú ê ú=
& 1 )2 û êë -m2gcos(v2 ) úû
(4.7.40)
ë 2 2 1 m2 û ë 2 û ë m2sin(v2 - v1 )(v
which is identical to Eq. (4.7.7)
______________________________________________________________________________
The fact that agreement is achieved in Example 4.7.4 with the results of Section 4.7.1 is
not the point here. The takeaway is that matrix manipulations using just the terms leading to Eqs.
(4.7.40) can be carried out on the computer to form the ODE of motion for numerical integration,
without the ad-hoc manipulations of example 4.7.1. This observation is the basis for a general
ODE formulation and numerical methods for solution of the equations of multibody dynamics in
Chapter 5, using the tangent space parameterization formulation of Section 3.5.2.
It is not possible in all applications to find Lagrangian generalized coordinates that hold
over the full range of motion of the system. It is often possible, however, to define a family of
parameterizations q = yi (vi ) such that F(y i (v i )) = 0 for v i Î Vi , where Vi are open sets such
that the union of U = y i (V ) Ì C% covers the constraint manifold of Eq. (4.7.37); i.e.,
i i
C% = È i y i (Vi ) and (y i (v i ),Vi ) are called charts that comprise an atlas that covers the constraint
manifold C% (Dundas, 2018). The differential equation of Eq. (4.7.39) is formed and solved until
criteria dictate transition to a neighboring chart on which a new differential equation is formed
and solved. This yields a continuation of the solution that is shown schematically in Fig. 3.6.2.
This approach is adopted in Chapter 5 to create a broadly applicable ODE formulation for
multibody dynamics.
295
4.7.3 Theory of ODE
A first order initial-value problem in unknown y Î R n is
y& = f ( y, b, t)
(4.7.41)
y (t 0 ) = y 0
where f (y, b, t) Î R n is assumed to be a continuously differentiable function of y, b Î R k , and t

{
on the set D = y, b, t : 0 < y - y 0 £ a, 0< b - b0 £ c, 0 £ t - t 0 £ d . The vector b contains }
problem data such as design variables that define the system being modeled. While the
assumption of continuous differentiability of f ( y , b, t) can be relaxed slightly to Lipschitz
continuity (Teschl, 2012), the associated level of mathematical complexity is bypassed here.
Results that are proved in texts on the theory of ODE; e.g., (Teschl, 2012), include the following:
ODE Existence Theorem: With the foregoing hypotheses, there exist α > 0 , β > 0 , and
a unique solution y(t, b) of Eq. (4.7.41) on 0 < t - t 0 £ a and 0 < b - b 0 £ b that is
continuously differentiable with respect to b and t.
While the ODE existence theorem does not specify the magnitudes of nonzero parameters
a and b , continuation concepts such as illustrated in Fig. 3.6.2 show that the solution may be
extended until the hypotheses of the theorem are violated.
A second order initial-value problem in unknown v Î R n is
v = g ( v , v& , b , t)
&&
v (t 0 ) = v 0 (4.7.42)
v& (t 0 ) = v& 0
with the same hypotheses as in the first order problem of Eq. (4.7.41). Defining
T
u1 = v, u 2 = v& = u& 1 , and u = éëu1T u T2 ùû , Eq. (4.7.42) may be written as the first order initial-
value problem
T
u& = F (u, b, t) º éëu T2 g T (u1 , u 2 , b, t) ùû
T
(4.7.43)
u(t 0 ) = u 0 º éëu10T u 0T
2 û
ù
Applying the ODE Existence Theorem to Eq. (4.7.43), the equivalent problem of Eq. (4.7.42)
has a unique solution v(t, b ) that is twice continuously differentiable with respect to t and
continuously differentiable with respect to b.
The first and second order initial-value problems of Eqs. (4.7.41) and (4.7.42), with the
stated hypotheses, are well posed, in that they have unique solutions that depend continuously on
problem data.
While the ODE existence theorem guarantees there is a unique solution of the initial-
value problem in a neighborhood of the initial values, it does not say anything about the nature of
the solution. For example, the system may possess highly nonlinear components such as stiff
296
vehicle suspension rebound stops that become active almost instantaneously in an extreme
maneuver, exciting high frequency and highly damped response over a short period of time. Such
behavior is characterized by low frequency smooth behavior most of the time and extreme highly
damped high frequency response that must be reacted to and accounted for by solution methods
over a short period of time and practically ignored after the amplitude of high frequency
response has dissipated. Behavior of this sort occurs in well-designed mechanical systems that
encounter moderately violent events, react in a controlled way, and return to a more normal
mode of operation. Systems with behavior such as this are called stiff, or are said to exhibit stiff
behavior. While there is no rigorous definition of a stiff system, the above noted modes of
dynamic response let one recognize stiff behavior when it is encountered. A more practical
criterion for a stiff system is that explicit numerical integration methods fail when assigned the
task of simulating motion of a stiff system. This criterion is addressed in Section 4.8.
Variational equations of motion of three simple multibody systems, two planar and one spatial,
are written in terms of Lagrangian generalized coordinates, yielding ODE of motion. The
analytical manipulations required in this process are shown to be intricate and ad-hoc.
An approach that generalizes the procedure used in the examples is outlined and applied to one
of the examples. It is shown that analytical implementation of the approach is not likely to be
successful for general multibody systems, but the process can be continued on the constraint
manifold until singular behavior is encountered. If functions that define first and second order
ODE initial-value problems are smooth, the problems are well posed; i.e., unique solutions
exist that depend continuously on time and problem data.
297
4.8 Runge-Kutta Methods for Numerical Solution of ODE
A vast literature has evolved during the past century on numerical methods for solution of
first order ordinary differential equation (ODE) initial-value problems,
y& = f ( y ,t)
(4.8.1)
y(t 0 ) = y 0
where y = [ y1 K y m ] and the function f is continuously differentiable and bounded.

T
Numerical algorithms are generally based on approximating a solution on a discrete time grid t i ,
i=1, 2, ... until a desired final time tf is reached. The two most common methods are (1) Runge-
Kutta (RK) methods that are based on Taylor series approximation of the solution at time t n and
leading to an approximate solution at time step t n +1 and (2) backward differentiation (BDF)
methods that are based on interpolating solution data at t n and prior time steps and projecting to
an approximate solution at t n +1 . For a number of reasons associated with integration on the
kinematic constraint manifold, Runge-Kutta methods are employed in this text. Attempting to
cite even the most significant contributions to this field is a hopeless task. The most
comprehensive documentation of developments to date are two volumes by Hairer, et.al., (1993;
1996).
The focus of this section is on Runge-Kutta numerical methods that are well suited for
integration of the equations of mechanical system dynamics, with no attempt at deriving such
methods. References to the literature for properties of algorithms and their computer
implementation are considered adequate for the purposes of this text. Two distinctly different
approaches to numerical integration are explicit methods that use data at the current time step to
predict the solution at the next step and implicit methods that include the approximate solution at
the next time step in the numerical integration equations, thus requiring an iterative solution of
nonlinear equations. The former approach is attractive for its simplicity and accuracy for many
applications. The latter approach is more complex, but is applicable for solution of mechanical
systems with stiff behavior that defeat explicit methods.
Section 4.8.1 summarizes explicit and implicit Runge-Kutta methods that are applicable
for solution of first order ODE. Section 4.8.2 reduces second order differential equations to a pair
of first order equations and applies Runge-Kutta methods to the resulting first order equations.
The result is transformed back to the second order setting, yielding a family of methods for
solution of second order ODE. MATLAB computer codes that implement these numerical
integration methods are contained in files that accompany this text, use of which are presented in
Appendix 4.A.
4.8.1 Runge-Kutta Methods for First Order ODE
Runge-Kutta numerical integration methods have been well developed for the solution of
the first order ODE of Eq. (4.8.1), where y and f are m-vectors. Unless otherwise indicated,
results presented in this section are based on (Hairer, et. al.,1993;1996). Runge-Kutta methods
are one step methods that use no data prior to t n to approximate the solution at time step t n+1 ,
using formulas based on Taylor series approximation at t n . A broad range of RK integrators for
298
this problem can be written in the form of stage equations
æ i ö
K i = f ç t n + ci h,y n + h å a ijK j ÷ , i=1,...,s (4.8.2)
è j=1 ø
and the step solution equation
s
y n +1 = y n + h å bi K i (4.8.3)
i=1
i
where t n is the current time step; y n is the approximate solution at t n ; a ij , bi , and ci = å a ij
j=1
s
are constants; åb
i =1
i = 1 ; s is the number of stages in integrating from t n to t n +1 ; K i are stage
variables; and h is the integration step-size. It is important to note that stage variables are not
solution variables y or their derivatives.
A concise representation of data for a broad spectrum of RK methods is the Butcher
tableau of Table 4.8.1. In all cases treated here, a ij = 0 for j > i . If a ii = 0 for all i = 1, ...,s, then
K i does not appear on the right side of Eq. (4.8.2) and K i is explicitly determined by Eq. (4.8.2)
, as a function of known data, so the method is called explicit. If a ii ¹ 0 for any i, then K i
appears on both sides of Eq. (4.8.2), constituting implicit numerical integration. Since f is
virtually always a nonlinear function of its arguments, an iterative method for solving for the
stage variables in implicit methods is required.
Table 4.8.1. Butcher Tableau
ci a ij
c1 a 11 0 0 0
c2 a 21 a 22 0 0
M M O
cs a s1 a s2 L a ss
bi b1 b2 L bs
It is intuitively clear that an explicit method will have difficulty responding to an event
such as encounter of a stiff spring and associated damping, called a stiff event, until significant
error has accumulated. A brief discussion of situations in mechanical system dynamics that yield
stiffness is presented in Section 4.7.3. An implicit method, in contrast, must deal with the onset
of an extreme event at the instant it occurs. Variable step size mechanisms associated with
implicit methods enable immediate reduction of step size to accurately capture a stiff event and
to increase the step size after the amplitude of response has reduced below an established error
tolerance. This is a pseudo definition of stiffness as behavior that defeats explicit integration
methods and requires implicit integration methods. These concepts will become clear as
integration methods are presented in this section and used in subsequent chapters to treat systems
with stiff behavior.
299
4.8.1.1 Explicit Runge-Kutta Methods for First Order ODE
One of the most popular RK methods is of order 4, called Runge-Kutta4. It is specified by
the tableau of Table 4.8.2 (Hairer, Norsett, and Wanner, 1993, p. 138). Its local truncation error
is of order h 5 . Since all elements on the diagonal are zero, it is an explicit method. The stage
equations of Eq. (4.8.2), with data from Table 4.8.2 are
K1 = f ( t n , y n )
K 2 = f ( t n + h / 2, y n + (h/2)K 1 )
(4.8.4)
K 3 = f ( t n + h / 2, y n + (h/2)K 2 )
K 4 = f ( t n + h, y n + hK 3 )
The approximate solution at t n+1 is given by Eq. (4.8.3), with data from Table 4.8.2 ,
y n+1 = y n + (h / 6) ( K1 + 2K 2 + 2K 3 + K 4 ) (4.8.5)
Table 4.8.2. Runge-Kutta 4

ci a ij
0 0
1/ 2 1/ 2 0
1/ 2 0 1/ 2 0
1 0 0 1 0
bi 1/ 6 2 / 6 2 / 6 1/ 6
Explicit RK methods for first order ODE have been developed using embedded methods
that have the same coefficients a ij , but different bi . This leads to two approximate solutions of
different orders of accuracy, whose difference serves as an estimate of error in the underlying
method. The tableau of Table 4.8.1 for these methods is extended to the form of Table 4.8.3.
Table 4.8.3. Butcher Tableau for Embedded Method
ci a ij
c1 a 11 0 0 0
c2 a 21 a 22 0 0
M M O
cs a s1 a s2 L a ss
bi b1 b2 L bs
di d1 d 2 L ds
Given stage variables K i that satisfy Eq. (4.8.2), two approximate solutions are obtained using
Eq. (4.8.3),
300
s
y n +1 = y n + h å bi K i
i=1
s
(4.8.6)
yˆ n +1 = y n + h å di K i
i=1
The difference between the solutions, y n - yˆ n , is an estimate of solution error. A measure of

error at time t n is suggested as (Hairer, Norsett, and Wanner, 1993)
2
1 m æ y ni - yˆ ni ö
err = åç
m i=1 è sci ø
÷ (4.8.7)
where a desired bound on error is

sci = Atol + y ni Rtol (4.8.8)
and Atol and Rtol are absolute error tolerance and relative error tolerance set by the analyst.
Provided err < 1, satisfactory solution accuracy is declared. If p is the order of the underlying
method, its error is proportional to h p +1 (Hairer, Norsett, and Wanner, 1993). Assuming the same
constant of proportionality for the embedded method of order h p , an optimum step size is
h opt = h ´ (1 / err )
1/(p +1)
(4.8.9)
If, during the numerical integration process, err approaches one, a new step size based on Eq.
(4.8.9) is selected. Factors are introduced to assure that the new step size leads to stable results,
limiting the rate of decrease or increase of h.
One of the best known adaptive explicit RK methods is the Runge-Kutta-Fehlberg45
method, given by the tableau of Table 4.8.4 (Hairer, Norsett, and Wanner, 1993, p. 177). The
method has error bounded by h 5 and is able to take steps as large as possible, while controlling
error using Eqs. (4.8.6) to (4.8.9).
301
Table 4.8.4. Runge-Kutta-Fehlberg 45
ci a ij
0 0
1 1
0
4 4
3 3 9
0
8 32 32
12 1932 7296
7200
13 2197 - 2197 0
2197
1 439 3680 845
-8 - 0
1 216 513 4104
11 0
2 8 3544 1859 -
- 2 - 40
27 2565 4104
bi 25 1408 2197 1
0 - 0
216 2565 4104 5
di 16 6656 28561 9 2
0 -
135 12825 56430 50 55
4.8.1.2 Implicit Runge-Kutta Methods for First Order ODE
While it can be interpreted as a Runge-Kutta method (Ascher and Petzold, 1998), the
implicit trapezoidal integration formula (Atkinson,1989) is presented here in terms of
generalized coordinates, rather than stage variables,
y n = y n -1 + (h / 2) ( y& n -1 + y& n ) (4.8.10)
The trapezoidal formula is an order two, A-stable method. The important thing to point out about
it is that, although the formula is stiffly accurate, this does not result in L-stability (Hairer and
Wanner, 1996), so it has difficulty in treating stiff systems.
Numerical integration can be implemented with a singly diagonal implicit Runge-Kutta
(SDIRK) formula, in which all diagonal entries in the tableau have the same nonzero value
a ii = a ¹ 0 . For SDIRK formulas, the associated Butcher tableau assumes the form shown in
æ i ö
Table 4.8.5. The ith equation of Eqs. (4.8.2) is K i = f ç t n + ci h,y n + h å a ijK j ÷ , where K1 L K i-1
è j=1 ø
have been determined as solutions for i = 1L s . Since K i appears on both sides of this equation
and f (t,y ) is generally nonlinear, an iterative solution method for solving the equation, written
in residual form,
æ i ö
R (K i ) º K i - f ç t n + ci h,y n + h å a ijK j ÷ = 0 (4.8.11)
è j=1 ø
302
must be employed. With an estimate K ij and Jacobian R K i (K ij ) of the residual, Newton-
Raphson iteration is
R Ki (K ij )DK ij = ( I - hαfy (K ij ) ) DK ij = -R (K ij )
(4.8.12)
K ij+1 = K ij + DK ij
j = 0, 1,..., until convergence criteria are met. A reasonable estimate for K 0j is the solution from
the preceding stage and, for K 10 , y& n from previous integration result. The approximate solution
at t n+1 is then evaluated using Eq. (4.8.3). This class of methods has accuracy of order h s and
attractive stability properties, including the ability to successfully integrate stiff systems that
defeat explicit methods.
Table 4.8.5. Butcher’s Tableau for SDIRK Methods
ci a ij
c1 a
c2 a 21 a K
M M M O
cs a s1 a s2 L a
bi b1 b 2 L bs
The SDIRK formula selected for integrating potentially stiff differential equations should
be L-stable (Hairer and Wanner, 1996) and of average order. The L-stability attribute ensures
good stability properties and order-preservation, even for stiff problems. The formula commonly
chosen is of order p = 4 , with s = 5 stages, hence the name SDIRK54. This order is high enough
to ensure good efficiency for tolerances typically used in simulations of engineering application,
namely 10-2 to 10-5 . The stiffly-accurate, L-stable, 5 stage, order 4 SDIRK54 formula for first
order ODE, with imbedded error estimation, is defined in Table 4.8.6 (Hairer and Wanner, 1996,
p.100).
Table 4.8.6 SDIRK54
ci a ij
1/ 4 1/ 4
3/ 4 1/ 2 1/ 4
11 / 20 17 / 50 -1/ 25 1/ 4
1/ 2 371 / 1360 -137 / 2720 15 / 544 1/ 4
1 25 / 24 -49 / 48 125 / 16 -85 /12 1/ 4
bi 25/24 - 49/48 125/16 - 85/12 1/4
di 59/48 - 17/96 225/32 - 85/12 0
303
4.8.2 Runge-Kutta Methods for Second Order ODE
In order to adapt Runge-Kutta methods such as those presented in Section 4.8.1 for first
order ODE for integration of second order ODE, the second order equation is first reduced to a
pair of first order equations. The method of Eqs. (4.8.2) and (4.8.3) is then applied and the result
is transformed back to the second order setting, as a second order integration algorithm.
For the second order initial-value problem in v Î R m and t,
v = g ( t,v, v& )
&&
v (t 0 ) = v 0 (4.8.13)
v& (t ) = v&
0 0
define the pair of variables

u=v
(4.8.14)
w = v& = u&
The second order equation of Eq. (4.8.13) is thus equivalent to the system of first order equations
T
in y º éë u T w T ùû ,
é u& ù é w ù
y& = ê ú = ê º f (t, u, w )
& û ë g ( t,u, w ) úû
(4.8.15)
ëw
T
Applying Eq. (4.8.2), with K j = éëku jT kw jT ùû and
é i
ù
i ê u n + h å a ijku j ú
y n + h å a ijK j = ê ú
j=1
(4.8.16)
ê i ú
ê w n + h å a ijkw j ú
j=1
ë j=1 û
the stage equations for the ODE of Eq. (4.8.15) are
é i
ù
ê w n + h å a i jkw j ú
é ku i ù ê j=1
ú
êkw ú = ê æ ö ú
(4.8.17)
ë iû ê
i i
g ç t n + ci h,u n + h å a i jku j , w n + h å a i jkw j ÷ ú
êë è j=1 j=1 ø úû
In component form, this is
i
kui = w n + h å a i jkw j (4.8.18)
j=1
æ i i ö
kw i = g ç t n + ci h,u n + h å a i jku j , w n + h å a i jkw j ÷ (4.8.19)
è j=1 j=1 ø
304
j
Writing Eq. (4.8.18) in the form ku j = w n + h å a jl kw l , the upper limit of summation can be
l =1
changed to s, because if l > j , then a jl = 0 . Substituting this into the second argument on the
right of Eq. (4.8.19),
æ i
æ s
ö i ö
kwi = g ç t n + ci h,u n + h å a i j ç w n + h å a jlkw l ÷ , w n + h å a i jkw j ÷
è j=1 è l =1 ø j=1 ø
(4.8.20)
æ æ i ö s s i ö
= g ç t n + ci h,u n + h ç å a i j ÷ w n + h åå a i ja jlkw l , w n + h å a i jkw j ÷
2
ç ÷
è è j=1 ø j=1 l =1 j=1 ø
where the upper limit of summation over j is changed to s, since if j > i, then a i j = 0 . Define
s
Ail = å a i ja jl (4.8.21)
j=1
or in matrix form, A = éë A ij ùû = aa , where a = éë a i j ùû . The only nonzero terms in the sum of Eq.
(4.8.21) are with j £ i and l £ j ; i.e., with l £ i . Thus, if l > i , either a i j = 0 or a jl = 0 , so
Ail = 0 and the matrix A has zeros above the diagonal, just as a .
These manipulations lead to the stage equation of Eq. (4.8.20) for the second order
differential equation of Eq. (4.8.13), in terms of only stage variable kw i ,
æ i i ö
kw i = g ç t n + ci h,u n + hci w n + h 2 å A ilkw l , w n + h å a i jkw j ÷ (4.8.22)
è l =1 j=1 ø
æ i ö
where ci = ç å a i j ÷ and A ij = 0 if j > i, so the upper limit of summation is changed from s to i.
è j=1 ø
Using Eq. (4.8.3), the approximate solution of Eq. (4.8.15) is
é u n +1 ù é u n ù s
é kui ù
ê w ú ê w ú å bi êkw ú
= + h (4.8.23)
ë n +1 û ë n û i =1 ë iû
or in component form
s
u n+1 = u n + h å b jku j
j=1
s
(4.8.24)
w n+1 = w n + h å b jkw j
j=1
Substituting from Eq. (4.8.18) into the first of Eqs. (4.8.24) yields
305
s
æ j
ö
u n+1 = un + h å b j ç w n + h å a jl kw l ÷
j=1 è l =1 ø
æ s ö s s
= un + h ç å b j ÷ w n + h 2 åå b ja jl kw l (4.8.25)
è j=1 ø j=1 l =1
s
= un + hw n + h 2 å Bl kw l
l =1
s
where åb
j=1
j = 1 has been used, the upper limit of summation on the right has been increased to s,
for the usual reason, and Bl is defined as

s
Bl = å b ja jl (4.8.26)
j=1
or in matrix form, B = ba .
Returning to the variables v and v& , using Eqs. (4.8.14) to eliminate u and w, and defining
k i = kwi , yields the second order stage equation of Eq. (4.8.22) for Runge-Kutta integration of
second order ODE,
æ i i ö
k i = g ç t n + ci h,v n + hci v& n + h 2 å A i jk j , v& n + h å a i jk j ÷ (4.8.27)
è j=1 j=1 ø
and, from Eqs. (4.8.24) and (4.8.25), the approximate solution at t n+1 is
s
v n+1 = v n + hv& n + h 2 å B jk j
j =1
s
(4.8.28)
v& n+1 = v& n + h å b jk j
j=1
The tableau of Table 4.8.1 is extended to represent RK formulas for second order ODE as Table
4.8.7. It contains all data needed to implement Eqs. (4.8.27) and (4.8.28).
Table 4.8.7. Runge-Kutta Tableau for Second Order ODE
ci a ij A ij
c1 a 11 0 0 0 A11 0 0 0
c2 a 21 a 22 0 0 A 21 A 22 0 0
M M O M O
cs a s1 a s2 L a ss As1 A s2 L A ss
bi ® b1 b2 L bs B1 B2 L Bs ¬ Bi
306
If any a ii ¹ 0 in Table 4.8.7, the unknown k i appears on both sides of Eq. (4.8.27) and
the method is implicit. Nonlinear discretized equations for the ODE of Eq. (4.8.27) must then be
solved for k i , i = 1,...,s . To focus on the solution variable k i in the ith equation, where k j ,
j = 1,...,i - 1 , have been determined by the first i - 1 of Eqs. (4.8.27), define
i -1
v¢i = v n + hci v& n + h 2 å A ijk j
j=1
i -1
(4.8.29)
v& ¢i = v& n + h å a ijk j
j=1
Equation (4.8.27) may thus be written as

k i = g ( t n + ci h,v¢i + h 2 A iik i ,v& ¢i + ha ii k i ) (4.8.30)
This form of the equation shows clearly the dependence on k i and identifies terms that do not
vary during iteration for k i . In particular, if a ii = Aii = 0 , i = 1,...,s, the right side of Eq. (4.8.30)
does not depend on k i and the associated method is explicit.
4.8.2.1 Explicit Runge-Kutta Methods for Second Order ODE
Several explicit RK methods are used in solving second order ODE. A few of the most
common are presented here. For calculation with a ii = Aii = 0 , i=1,...,s, in solving Eq. (4.8.13),
Eq. (4.8.30) yields
k i = g ( t n + ci h,v¢i ,v& ¢i ) (4.8.31)
An explicit RK method for solving second order differential equations is the order 4 Nystrom4
method (Hairer, Norsett, and Wanner, 1993, p.284) that is characterized by the tableau of Table
4.8.8. Note that A ij in Table 4.8.8 do not satisfy Eq. (4.8.21).
Table 4.8.8. Nystrom4

ci a ij A ij
0 0 0
1/ 2 1/ 2 0 1/8 0
1/ 2 0 1/ 2 0 1/8 0 0
1 0 0 1 0 0 0 1/ 2 0
bi ® 1/ 6 2 / 6 2 / 6 1/ 6 1/ 6 1/ 6 1/ 6 0 ¬ Bi
307
Stage equations of Eq. (4.8.31) for this method, in solving Eq. (4.8.13), are
k1 = g ( t n ,v n ,v& n )
æ h h h2 h ö
k 2 = g ç t n + ,v n + v& n + k1 ,v& n + k1 ÷
è 2 2 8 2 ø
æ h h h2 h ö (4.8.32)
k 3 = g ç t n + ,v n + v& n + k 1 ,v& n + k 2 ÷
è 2 2 8 2 ø
æ h2 ö
k 4 = g ç t n + h,v n + hv& n + k 3 ,v& n + hk 3 ÷
è 2 ø
The approximate solution at t n +1 , from Eqs. (4.8.28), is
h2
v n +1 = v n + hv& n + ( k1 + k 2 + k 3 )
6 (4.8.33)
h
v& n +1 = v& n + ( k 1 + 2k 2 + 2k 3 + k 4 )
6
The second order ODE tableau of Table 4.8.9 is based on the RK4 tableau of Table 4.8.2
and satisfies Eq. (4.8.21). The tableau of Table 4.8.8, however, has yielded equally accurate
results on problems with known solutions.
Table 4.8.9. Second Order Runge-Kutta4
ci a ij A ij
0 0 0
1/ 2 1/ 2 0 0 0
1/ 2 0 1/ 2 0 1/ 4 0 0
1 0 0 1 0 0 1/ 2 0 0
bi ® 1/ 6 2 / 6 2 / 6 1/ 6 1/ 6 1/ 6 1/ 6 0 ¬ Bi
To create a second order ODE integrator with error control, based on the Runge-Kutta-
Fehlberg45 method of Section 4.8.1.1, parameters A i j are defined by Eq. (4.8.21), second order
solution evaluation parameters Bi are defined by Eq. (4.8.26) , and embedded error estimation
parameters Di are defined as
s
Di = å d ja ji (4.8.34)
j=1
The resulting tableau is presented in Table 4.8.10, called Runge-Kutta-Fehlberg-Nystrom45, or

RKFN45, to reflect the contributing components. Numerical values for A i j , Bi , and Di are given
to four places to illustrate their orders of magnitude. Numerical values computed from Eqs.
308
(4.8.21), (4.8.26), and (4.8.34) that are accurate to computer precision are used in
implementation of the algorithm.
Table 4.8.10. Runge-Kutta-Fehlberg-Nystrom45
For the second order equation of Eq. (4.8.13), with stage values k i of Eq. (4.8.27), the
first solution is evaluated in Eq. (4.8.28) and the second by its analog,
s
vˆ n+1 = v n + hv& n + h 2 å D jk j
j=1
s
(4.8.35)
vˆ& n+1 = v& n + h å d jk j
j=1
T T
Since y = éë u T w T ùû = éë v T v& T ùû ,
scvi = Atol + v ni Rtol

(4.8.36)
scv& i = Atol + v& ni Rtol
and Eq. (4.8.7) is
1 éêæ m æ vi - vˆ i ö v& i - vˆ& i ö ö ùú

2 2
m æ
err = çåç ÷ + åç ÷ ÷ (4.8.37)
2m êç i=1 è scvi ø i=1 è scv& i ø ÷ ú
ëè øû
Step size control thus follows Eq. (4.8.9).
309
4.8.2.2 Implicit Runge-Kutta Methods for Second Order ODE
Since one or more of the Runge-Kutta coefficients a ii is nonzero in an implicit method,
Eqs. (4.8.27) must be solved numerically, usually with an iterative Newton-Raphson method.
The implicit trapezoidal method can be viewed as a Runge-Kutta method (Ascher and
Petzold,1998). To take advantage of its form in terms of generalized coordinates, rather than
stage variables, it is presented as an extension of Eq. (4.8.10). Writing Eq. (4.8.10) for both v and
v& ,
v n = v n -1 + (h / 2) ( v& n -1 + v& n )
(4.8.38)
v& n = v& n -1 + (h / 2) ( && vn )
v n -1 + &&
Substituting the second equation into the first yields a second order formula,
v n = v n -1 + (h / 2) ( v& n -1 + v& n -1 + (h / 2) ( && vn ) )
v n -1 + &&
(4.8.39)
= v n -1 + hv& n -1 + (h 2 / 4) ( && vn )
v n -1 + &&
With the second of Eqs. (4.8.38), this yields the second order trapezoidal formula
v n = v n -1 + hv& n -1 + (h 2 / 4) ( &&v n -1 + &&
vn )
(4.8.40)
v& n = v& n -1 + (h / 2) ( && vn )
v n -1 + &&
The trapezoidal formula is an order two, A-stable method that is often used and is
analyzed in detail by Atkinson (1989). The important thing to point out about it is that, although
the formula is stiffly accurate, this does not result in L-stability, hence it is expected to have
difficulties with stiff systems.
Numerical integration can be implemented with a singly diagonal implicit Runge-Kutta
(SDIRK) formula, in which all diagonal entries have the same nonzero value, a ¹ 0 . For SDIRK
formulas, the Butcher tableau assumes the form of Table 4.8.5.
For first order SDIRK formulas, a ii = a ¹ 0, i = 1,...,s . The matrix a = éëa ij ùû given in
Table 4.8.5 is the coefficient matrix of the formula. For SDIRK methods a is nonsingular. Note
that the diagonal terms in the second order tableau of Eq. (4.8.21), A = éë A ij ùû = aa , are
s
Aii = å a ija ji = ( a ii ) = a 2 , since if i ¹ j , one of the terms in the product forming the sum is
2
j=1
zero, leaving only the product with j = i . Thus, the second order method is also singly diagonal.
In residual form, Eq. (4.8.30) is
R = k i - g ( t n + ci h,v¢i + h 2 a 2 k i ,v& ¢i + hak i ) = 0 (4.8.41)
The Jacobian in iterative solution for k i is thus
R k i º J = I - h 2a 2g v - hag v& (4.8.42)
310
whose form does not depend on i. Since the arguments of g vary little over the stages, i = 1,...,s ,
and even over several time steps, the Jacobian of Eq. (4.8.42) can be held fixed over all stages,
and possibly over several time steps, until excessive iterations are encountered in solving Eq.
(4.8.41), using a method such as Newton-Raphson iteration. Once Eqs. (4.8.41) are solved for
k i , i = 1,...,s , the approximate solution at t n +1 = t n + h is given by Eqs. (4.8.28).
As in the case of first order ODE, the SDIRK formula sought for integrating potentially
stiff differential equations should be L-stable, and of average order. The 5 stage, order 4
SDIRK54 formula for first order ODE defined in Table 4.8.6 has this property. From Eqs.
(4.8.21) and (4.8.26), the coefficients for the second order ODE in Tableau 4.8.7, to five place
accuracy, are
é 0.0625 0 0 0 0 ù
ê 0.25 0.0625 0 0 0 ú
ê ú
A = ê 0.15 -0.02 0.0625 0 0 ú
ê0.12058 -0.02628 0.01378 0.0625 0 úú
ê
ëê0.73437 -0.46614 3.71093 -3.54166 0.0625ûú
B = [0.73437 - 0.46614 3.71093 - 3.54166 0.0625] (4.8.43)
D = [0.67708, 0.03125, 1.5625, -1.77083, 0]
where D contains coefficients for error estimation from Eq. (4.8.34). In applications, Eqs.
(4.8.21), (4.8.26), and (4.8.34) are used, with precise values of Table 4.8.6, to evaluate
A, B, and D for use in implementing numerical integration.
4.8.3 Generalized Form of Second Order ODE
As seen in examples presented in Section 4.7, second order ODE arising in system
dynamics are generally of the form
v = g (t,v,v& )
M ( v ) && (4.8.44)
where the coefficient matrix M ( v ) is nonsingular. Multiplying by the inverse of the coefficient
matrix yields the standard form
v = M -1 ( v)g(t,v,v& )
&& (4.8.45)
Applying the Runge-Kutta discretization of Eq. (4.8.30) yields
k i = M -1 ( v¢i + h 2 Aiik i )g ( t n + ci h,v¢i + h 2 A ii k i ,v& ¢i + ha ii k i ) (4.8.46)
Multiplying by M ( v ¢i + h 2 A ii k i ) and writing the result in residual form yields
R º M ( v¢i + h 2 Aii k i )k i - g ( t n + ci h,v¢i + h 2 A iik i ,v& ¢i + ha iik i ) = 0 (4.8.47)
The Jacobian in iterative solution for k i is
(( ) - g ) - ha g
R k i º M + h 2 A ii Mkˆ i
v
v ii v& (4.8.48)
311
where A ii = a 2 and a ii = a in the case of SDIRK methods.
Similarly, for the trapezoidal formula of Eq. (4.8.38), the residual equation is
R º M ( v n -1 + hv& n -1 + (h 2 / 4) ( && v n ) ) &&
v n -1 + && vn
(4.8.49)
-g ( t n ,v n -1 + hv& n -1 + (h 2 / 4) ( && v n ) ,v& n -1 + (h / 2) ( &&
v n -1 + && vn )) = 0
v n -1 + &&
and the Jacobian in iterative solution for &v&n is
(( ) - g ) - (h / 2)g
&&ˆ n
R &&v n = M + (h 2 / 4) Mv
v
v v& (4.8.50)
To implement both Eqs. (4.8.48) and (4.8.50), the term

M 2( v, μ) º ( M( v )μˆ ) v (4.8.51)
is coded and used with μ = k i in Eq. (4.8.48) and μ = &&

vn in Eq. (4.8.50)
4.8.4 Second Order Trapezoidal Method
The trapezoidal implicit method of Eq. (4.8.40)
v n = v n -1 + hv& n -1 + (h 2 / 4) ( && vn )
v n -1 + &&
(4.8.52)
v& n = v& n -1 + (h / 2) ( && vn )
v n -1 + &&
has properties that differ from most Runge-Kutta methods and deserves treatment in its own
right. The method and well-known variants such as the Newmark method (Newmark, 1959) and
the HHT- α method (Hilber, Hughes, and Taylor, 1977; Chung and Hulbert, 1993) that were
developed for use in structural dynamics are well suited for integration of the equations of
multibody dynamics (Negrut, Rampalli, Ottersson, and Sajdak, 2007; Negrut, Jay, and Khude,
2009). The trapezoidal method is treated in this section, and variants are treated in Chapter 7 for
integrating the differential-algebraic equation formulation of the equations of multibody
dynamics.
The trapezoidal method of Eq. (4.8.52) has attractive properties for use in multibody
dynamic simulation. It is simple in form, with variables of integration that are generalized
coordinates, as opposed to stage variables in Runge-Kutta methods that are not generalized
coordinates. As a result, it can be substituted directly into equations of motion to provide
discretized equations at each time step.
Disadvantages of the method include the fact it is only second order accurate, as opposed
to the higher order of accuracy of most Runge-Kutta methods. Perhaps more concerning is the
fact that the method lacks the degree of stability to be effective in treating stiff behavior that may
be encountered in multibody dynamics. Finally, there is no imbedded integration formula that
can be used for error and step size control.
4.8.5 Third Derivative for Variable Step Size Error Control
In dealing with the generalized form of second order ODE of Eq. (4.8.44),
v = g(t,v,v& )
M(v)&& (4.8.53)
312
an approximation for the third derivative of the solution &&& vn at time t n may be used to create an
error estimate for the solution v n+1 and v& n+1 . Using Taylor’s formula (Corwin and Szczarba,
1982),
v n+1 = v n + hv& n + (h 2 / 2)&&
v n + (h 3 / 6)&&&
vn + O(h 4 )
(4.8.54)
v& n+1 = v& n + h&&
v n + (h 2 / 2)&&&
v n + O(h 3 )
Time step control of Eqs. (4.8.37) and (4.8.9) may be used with these estimates; i.e.,
1 éæ nv æ vi - vi ö öù
2 2
nv
æ v& - v& i ö
err = êç å ç ÷ + åç i ÷ ÷ú
2nv êç i =1 è scvi ø i =1 è scv& i ø ÷ú (4.8.55)
ëè øû
h opt = h ´ (1/ err )
1/ (3)
where scvi = Atol + v ni Rtol and scv& i = Atol + v& ni Rtol .
A simple approximation of &&&

vn is the difference formula
v n = (1/ h) ( &&
&&& v n -1 )
v n - && (4.8.56)
While not necessarily accurate, this is an inexpensive way to implement error and step size
control. A better estimate may be had by differentiating Eq. (4.8.53) with respect to time to
obtain
( vˆ n ) v n + g (tˆn , v n , v&ˆ n ) v n v& n

v n = - (M ( v n ) &&
M ( v n )&&& ) (4.8.57)
v n + g (t n , vˆ n , v&ˆ n )t n
+ g(tˆ n , vˆ n , v& n ) v& n &&
The third derivative &&&

vn may thus be determined using one equation solution. While this may
appear to be a costly calculation, it may not be. Since the trapezoidal method is implicit, all terms
in Eq. (4.8.57) must be evaluated for iterative solution and the cost of factoring M( vn ) is less
than the cost of factoring the Jacobian and one evaluation of the residual in iterative solution of
the discretized equations of motion. Further, the estimate
v en+1 = &&
&& v n + h&&&
vn (4.8.58)
of the solution of the discretized equations may be accurate enough to avoid an occasional
iteration and the improved step size control associated with an accurate &&& vn should favorably
impact compute time. It should be noted that to implement the third derivative computation, the
term g t (t, v, v& ) , which is not required in any of the foregoing integrators, must be evaluated.
Similarly, explicit Runge-Kutta numerical integration methods such as the Nystrom4
method can take advantage of this error control approach. Since it requires four function
evaluations in each time step, the cost of the calculation of Eq. (4.8.57) may be acceptable,
provided large steps are achieved with error control.
313
Methods for numerical integration of ODE, primarily initial-value problems with first order
ODE, have been well-known for over a century. Especially for one step Runge-Kutta methods,
a deep theory of stability, error control, and accuracy has evolved. Numerical integration
formulas for first order ODE, both explicit that involve no iterative solution and implicit that
involve iterative solution, are summarized for six methods, two of which include error control.
A transformation of integration methods for first order ODE is presented that creates
algorithms for integration of second order ODE. A MATLAB implementation of six second
order methods is presented in Code 4.8 of Appendix 4.A. Components of Code 4.8 with which
the user interacts are presented in Section 4.9.1. Use of Code 4.8 is illustrated for solution of
multibody system examples in Section 4.9.2.
Key Formulas
y& = f (y, t) y (t 0 ) = y 0 (4.8.1)
æ i ö s
K i = f ç t n + ci h,y n + h å a ijK j ÷ , i=1,...,s y n +1 = y n + h å bi K i (4.8.2) (4.8.3)
è j=1 ø i=1
v = g ( t,v, v& ) v (t 0 ) = v 0 v& (t 0 ) = v& 0

&& (4.8.13)
v n = v n -1 + hv& n -1 + (h 2 / 4) ( && vn )
v n -1 + && v& n = v& n -1 + (h / 2) ( && vn )
v n -1 + && (4.8.40)
æ i i ö
k i = g ç t n + ci h,v n + hci v& n + h 2 å A i jk j , v& n + h å a i jk j ÷ (4.8.27)
è j=1 j=1 ø
s s
v n+1 = v n + hv& n + h 2 å B jk j v& n+1 = v& n + h å b jk j (4.8.28)
j=1 j=1
s s
Ail = å a i ja jl Bl = å b ja jl (4.8.21) (4.8.26)
j=1 j=1
314
4.9 Numerical Solution for Systems Governed by ODE
A brief user oriented outline of Code 4.8 of Appendix 4.A is presented in Section 4.A.1
of Appendix 4.A. Three examples of solutions obtained using tailored forms of Code 4.8 are
presented in Sections 4.9.2.
4.9.1 Code 4.8 for Second Order ODE Solution
MATLAB computer Code 4.8 of Appendix 4.A is comprised of six numerical integration
functions for solving the second order initial-value problem
v = g(t, v, v& )
M( v )&& (4.9.1)
v (0) = v 0
(4.9.2)
v& (0) = v& 0
Explicit solvers implemented are Nystrom4, RungeKutta4, Kuttta38, and RKFN45 of Section
4.8.2.1. Implicit solvers implemented are Trapezoidal and SDIRK54 of Section 4.8.2.2. Code 4.8
is intended to be a template of numerical solution methods that will be adapted for numerical
solution of numerous forms of second order ODE of mechanical system dynamics, rather than a
stand-alone code.
4.9.2 Numerical Examples of Systems Governed by ODE
To illustrate use of numerical integration methods presented in Section 4.8, using Code
4.8 of Appendix 4.A that is documented in Section 4.A.1 of Appendix 4.A, examples of Section
4.7 are studied. In each example, quantities that are required for implementation of numerical
integration methods are derived and implemented in a version of Code 4.8, identified as Codes
4.9.1 through 4.9.3 in Appendix 4.A.
4.9.2.1 Planar Double Pendulum
The equations of motion of Eq. (4.7.7) for the planar double pendulum with concentrated
masses of Section 4.7.1 are of the form of the second order ODE of Eq. (4.9.1); i.e.,
v = g (t,v,v& ) , with
M ( v ) &&
é (m1 + m 2 ) m 2 cos(v 2 - v1 ) ù
M(v) = ê ú
ë m 2 cos(v 2 - v1 ) m2 û
(4.9.3)
é -(m1 + m 2 )gcos(v1 ) ù é - m 2 sin(v 2 - v1 )(v
& 2 )2 ù
g( t,v,&v) = ê -
ú ê ú
ë - m 2 gcos(v 2 ) û ë m 2 sin(v 2 - v1 )(v & 1 )2 û
Data that must be entered in the user component of Code 4.8, to create Code 4.9.1 for this
example, are presented in Fig. 4.9.1. To assist in writing code in terms of parameters m1 and m2 ,
the function AdatPart is adapted in Code 4.8 for this application. In the present example, the
variables are simple enough that the parameter partitioning function would not be necessary, but
it is used as an illustration in Fig 4.9.4.
315
1 %4.9.1 Planar Double Pendulum; Based on Code 4.8, Second Order ODE Solver
2 intol=10^-8; %Tolerance in solving discretized equations of motion
3 Atol=10^-8; %Absolute error tolerance for variable step metnods
4
5 h=0.0001; %Step size
6 hmax=0.001; %Maximum Allowable Step size
7 hvar=1; %hvar=1, variable step;hvar=2, fixed step
8
9 tfinal=6; %Final time
10
11 %Explicit Integration Methods:
12 %Integ=1, Nystrom4; Integ=2, RungeKutta4;
13 %Integ=4, Kutta3/8; integ=4, RKFN
14 %Implicit Integration Methods:
15 %Integ=5, Trapezoidal; Integ=6, SDIRK54
16 Integ=6; %Numerical Integration Method Selected
17
18 nv=2; %Use variable dimension
19
20 %Fixed Parameter Data List-Complete Partitioning in function parPart if
21 %explicit use of variable names is desired in user supplied functions.
22 par=[nv;intol;Atol;hmax;hvar];
23 % Problem Data
24 m1=10;
25 m2=10;
26 K1=10000000;
27 C1=20000;
28 K2=10^10;
29
30 %Problem Parameter Data List-Partitioning in function AdatPart if
31 %use of variable names is desired in user supplied functions.
32 dat=[m1;m2;K1;C1;K2];
33
35 V=zeros(nv,10);
36 Vd=zeros(nv,10);
37 Vdd=zeros(nv,10);
38
39 % Initial Conditions
40 v0=[0;0]; %User define initial position
41 vd0=[0;0]; %User define initial velocity
42 V(:,1)=v0;
43 Vd(:,1)=vd0;
Figure 4.9.1 Code 4.9.1 Simulation Parameters, Double Pendulum
If only explicit integration is to be used, expressions of Eq. (4.9.3) are entered in
functions Agf and AM of Fig. 4.9.5. If implicit integration is to be used, terms in Eqs. (4.8.49),
(4.8.51), and (4.8.52) are
316
é (m + m 2 )gsin(v1 ) - m 2 cos(v 2 - v1 )(v& 2 ) 2 m 2cos(v 2 - v1 )(v& 2 ) 2 ù
gv = ê 1 2ú
ë m 2 cos(v 2 - v1 )(v& 1 ) 2
m 2 gsin(v 2 ) - m 2 cos(v 2 - v1 )(v& 1 ) û
é 0 2m 2sin(v 2 - v1 )v& 2 ù
g v& = ê ú (4.9.4)
ë -2m 2 sin(v 2 - v1 )v& 1 0 û
ém -m 2 ù
M 2( v, μ ) = sin(v 2 - v1 )m 2 ê 2
ë m1 -m1 úû
These terms are entered in functions Agfsvdd and AM2 of Fig 4.9.4.
1 function [m1,m2]=AdatPart(dat)
2
3 %User define elements in array dat by name, as in BparPart
4
5 m1=dat(1);
6 m2=dat(2);
1 function g=Agf(t,v,vd,par,dat)
2
3 [nv,intol,Atol,hmax,hvar]=BparPart(par);
4 [m1,m2]=AdatPart(dat);
5 v1=v(1);
6 v2=v(2);
7 vd1=vd(1);
8 vd2=vd(2);
9 %Enter right side of ODE, Eq. (4.7.7)
10
11 g=[-(m1+m2)*9.8*cos(v1);-m2*9.8*cos(v2)]-...
12 [-m2*sin(v2-v1)*(vd2^2);m2*sin(v2-v1)*(vd1^2)];
1 function [gsv,gsvd]=Agfsvvd(t,v,vd,par,dat)
2
5 v1=v(1);
6 v2=v(2);
7 v1d=vd(1);
8 v2d=vd(2);
9
10 %Enter derivatives of g with respect to v and vd
11 s1=sin(v1);
12 s2=sin(v2);
13 c2m1=cos(v2-v1);
14 s2m1=sin(v2-v1);
15
16 gsv=[(m1+m2)*9.8*s1-m2*c2m1*v2d^2,m2*c2m1*v2d^2;...
17 m2*c2m1*v1d^2,m2*9.8*s2-m2*c2m1*v1d^2];
18 gsvd=[0,2*m2*s2m1*v2d;-2*m2*s2m1*v1d,0];
1 function M=AM(v,par,dat)
2
317
5 v1=v(1);
6 v2=v(2);
7
8 % Enter Mass Matrix M=M(v,par)
9
10 M=[m1+m2,m2*cos(v2-v1);m2*cos(v2-v1),m2];
1 function M2=AM2(v,mu,par,dat)
2
5 v1=v(1);
6 v2=v(2);
7 mu1=mu(1);
8 mu2=mu(2);
9
10 % Enter M2=(M(q,par)mu)sq
11
12 M2=sin(v2-v1)*m2*[mu2,-mu2;mu1,-mu1];
Figure 4.9.4 Code 4.9.1 User Input Functions, Double Pendulum
With m1 = m2 = 1 kg, plots of v1 and v2 in Fig 4.9.2 obtained using a constant step size
of h = 0.001 sec with the Nystrom4 explicit integrator indicate a relatively smooth swinging
motion. Total energy for this conservative system was constant to within a maximum variation of
2e-9. Essentially identical results were obtained with all six integrators. Variable step size
simulations using the RKNF45 and SDIRK54 integrators with a maximum allowed step size
hmax = 0.01 sec yielded comparable results, with step sizes near the upper limit.
Figure 4.9.2 vi vs t, Double Pendulum, m1 = m2 = 1 kg, h = 0.001 sec

To test the strength of the numerical integration formulations, mass 1 is held at unity and
mass 2 is set to 100 kg. Numerical results presented in Fig. 4.9.7 were obtained using SDIRK54
with a constant step size h = 0.0001. They show that moderately high frequency oscillation
318
occurs. With constant step size SDIRK54, the variation in total energy was less than 7e-5,
whereas with the explicit Nystrom4 integrator the variation was 4e-4. Allowing variable step
size, with an upper bound hmax = 0.01, SDIRK54 chose step sizes ranging from just below
0.001 to 0.01, with results essentially identical to those of Fig. 4.9.3. However, thevariation in
total energy grew to 9e-3.
Figure 4.9.3 vi vs t, Double Pendulum, m1 = 1, m2 = 100 , h = 0.001 sec

4.9.2.2 Planar Slider-Crank
The equations of motion of Eq. (4.7.14) for the planar slider-crank of Section 4.7.2, with
connecting rod length 2 m and crank radius r m, are of the form of the ODE of Eq. (4.8.45); i.e.,
v = g (t,v,v& )
M( v )&& (4.9.5)
with
M = J1 + m 2 A 2
(4.9.6)
g = -m 2 ABv& 2
where
R 2sin(v)cos(v)
A º Rsin(v) +
4 - (Rsin(v))2
R 2 (cos(v)2 - sin(v)2 ) R 4 (sin(v)cos(v))2
A v = B º Rcos(v) + +
( 4 - (Rsin(v)) )
3/2
4 - (Rsin(v))2 2
If only explicit numerical integration is to be used, expressions of Eq. (4.9.6) are entered in
functions Agf and AM in Code 4.9.2 of Appendix 4.A.
If implicit integration is to be used, terms in Eqs. (4.8.49), (4.8.51), and (4.8.52) are
g v = - m 2 A 2 v& 2 - m 2 BCv& 2
g v& = -2m 2 ABv& (4.9.7)
M2(v,m) = 2m 2mAB
319
where
3R 6 ( sin(v)cos(v) )
3
2R 2 sin(v)cos(v) 3R 4 (cos(v)2 - sin(v) 2 )sin(v)cos(v)
C º - Rsin(v) - + +
( 4 - (Rsin(v))2 ) 2 ( 4 - (Rsin(v)) 2 )
3/ 2 5/ 2
4 - (Rsin(v))2
These terms are entered in functions Agfsvdd and AM2 of Code 4.9.2. Rather than including
user input files in the text, as was done in Section 4.9.2.1, the reader is referred to Code 4.9.2 in
Appendix 4.A for details.
As an initial simulation, parameters are set as R=1.5 m, J1 = 20 kg × m2 , m2 = 20 kg, and
g = 9.8m / sec2 . Plots of crank angular velocity v& = vd and x 2 = x2 and its derivatives are
presented in Fig. 4.9.4, obtained using the explicit Nystrom4 integrator, with an initial crank
angular velocity of 10 rad/sec and step size h = 0.001 sec. Expressions for slider position x 2 and
its derivatives, as functions of v and its derivatives, are given by Eqs. (4.7.9) and (4.7.10).
Comparable results were obtained using all integrators and kinetic energy varied only 0.01%
over the simulation period for this conservative system.
x 2 for Slider Crank, R=1.5, m 2 = 20, w0 = 10 , h = 0.001

& x 2 , x& 2 , and &&
Figure 4.9.4 v,
As shown in Example 3.7.1, the slider-crank has a singularity if R ³ 2 . Setting R = 1.98,
retaining the remaining data above, yields simulation results for crank angular velocity and &x& 2 ,
using the SDIRK54 integrator with h = 0.001, shown in Fig. 4.9.5. As noted, substantially larger
accelerations, hence larger associated bearing forces, occur.
320
Figure 4.9.5 && x 2 for Slider Crank, R=1.98, m 2 = 20, w0 = 10 , h = 0.001
v and &&
The observation in Chapter 1 that nonlinearity in machine kinematics and dynamics is
both a blessing and a curse is borne out with the slider crank. This simple mechanism makes
possible the internal combustion engine that has been at the heart of automotive and
manufacturing technology for well over a century. Its nonlinear kinematic behavior makes
possible its monumental contributions. Its nonlinear dynamic behavior, including large
accelerations and associated loads, makes dynamic analysis and design of bearings for long life a
challenge. As with many aspects of engineering, one must overcome such challenges to take
advantage of benefits offered by nonlinear mechanical systems
4.9.2.3 Spatial Robotic Manipulator
The equations of motion of Eqs. (4.7.29) and (4.7.30) for the spatial robotic manipulator
of Section 4.7.3 are of the form of the second order ODE of Eq. (4.8.45); i.e.,
v = g (t,v,v& )
M ( v ) && (4.9.8)
with v Î R 3 and
M (v ) = W1T J1¢ W1 + m 2 V2T V2 + 2 W2T J ¢2 W2 + m3 V3T V3
²v& )J¢ W v& + m V T ( U + gu ) + 2 W T J¢ X ö
æ W1T ( W (4.9.9)
g(v,v) = - ç ÷
1 1 1 2 2 2 z 2 2 2
&
ç +2 W ( W
T ²v& )J ¢ W v& + m V ( U + gu )
T ÷
è 2 2 2 2 3 3 3 z ø
If only explicit integration is to be used, expressions of Eq. (4.9.9) are entered in functions Agf
and AM in Code 4.8 of Appendix 4.A and one of the explicit integrators is invoked to obtain a
numerical solution.
If implicit integration is to be used, terms in Eqs. (4.8.49), (4.8.51), and (4.8.52) must be
calculated. Since Ui (v,v& ) and Xi (v,v& ) are column vectors, derivatives Uiv , Uiv& , Xiv , and Xiv& can
be evaluated. Since Vi (v) and Wi (v) are matrices, derivatives
1 ( i ) v
i v
( i )
( V αˆ ) , Wβˆ , ( V T γˆ ) , and W Tδˆ are to be evaluated.
v v
To evaluate ( M (v )μˆ ) v , the fact that each of the four terms in the first of Eqs. (4.9.9) is
symmetric leads to
321
(V i
T
(
Vμ ) + ( V Vˆ μˆ ) = V ( V μˆ ) + + ( Vˆ Vμˆ ) = 2V ( V μˆ )
i ) = Vi Vi μ
ˆ ˆT ˆ
v v
i
T
i
v
i
T
i v i
T
i
v
i
T
i v
( W J¢W μˆ ) = ( Wˆ J¢W μˆ ) + ( W J¢Wˆ μˆ ) = W J¢ ( W μˆ ) + ( Wˆ J¢W μˆ )

i
T
i i v i
T
i i
v
i
T
i i
v
i
T
i i v i
T
i i
v
= 2 W J ¢i ( Wiμˆ ) v
i
T
Since ( W1μˆ ) v = 0 ,
M 2( v, μ) = 2m 2 V2T ( V2μˆ ) v + 2W2T J ¢2 ( W2μˆ ) v + 2m3V3T ( V3μˆ ) v (4.9.10)
i i i ( )
²v& )J¢W v& in g(v,v& ) require a bit of derivation, as follows:
The terms (W
v
( (W
²v& )J¢W v& )
i i i
v
²v& )J ¢ W v&ˆ
= (Wi i i ( ) v
²
(
+ (Wi v&ˆ )J¢i W
ˆ v&ˆ
i ) v
(4.9.11)
% of Eq. (2.1.27),
% = -ba
Using the identity ab
( (W²vˆ& )J¢Wˆ vˆ& ) = - (( J²

i i i ) )
ˆ vˆ& W vˆ&
¢W
v
i i i
v
= - J² (
¢i Wi v& Wi v&ˆ )( ) v
Thus, Eq. (4.9.11) becomes
((W
²v& )J ¢W v& )
i i i
v
i i i ) ( )( )
²v& )J ¢ W v&ˆ - J²
= (W (
¢i Wi v& Wi v&ˆ
v v
(4.9.12)
= ((W ¢W v& ) ) ( W v&ˆ )
²v& )J ¢ - ( J²
i i i i i
v
Since ( W1Taˆ ) = 0 ,
v
æ m VT U
ç 2 2 ( (
ˆ + gu
2 z
v
))ˆ
+ m 2 V2T U 2v + W2T J ¢2 X 2
v
+ W2T J ¢2 X 2v ( ) ö
÷
ç ÷
ç è
æ
è
2 2 (
²v& J¢ W v& ö÷ ö + 2W T ( W
·
g v = - ç +2 ç W2T æç W 2 ÷
ø øv
2)²v& )J ¢ - J²
2 2
¢2 W2 v& W2 vˆ& ( ( )) ( ) ÷
v÷
(4.9.13)
ç ÷
ç + m3 V3T U
è ( (
ˆ + gu
3 z
v
+ m3 V3T U 3 v )) ÷
ø
For dependence of g(v,v& ) on v& , using Eq. (2.1.26),
( (W
²v& )J ¢ W v& )
1 1 1
v&
( ²
) + ( (W²v& )J¢W vˆ& )
= ( W1 vˆ& )J1¢ W1v&
v&
1 1 1
v&
= (W 1 (( ¢W vˆ& ) W v& )
²v& )J¢ W - J²
1 1 1 1 1
v
(4.9.14)
(²v& )J¢ - J²
= (W1 1
¢ & W1
1 W1 v ( ))
Thus
æ WT W
ç 1
g v& = - ç
1 1 (( ) ( ))
²v& J¢ - J²¢ & W1 + m 2 V2T U 2v& + 2 W2T J ¢2 X2v& ö
1 W1 v
÷
(4.9.15)
ç +2W2T W
è 2 2 (( ) ( ))
²v& J ¢ - J²¢2 W2 v& W2 + m3 V3T U 3v&
÷
÷
ø
322
For this example, from Eqs. (4.7.21) and (4.7.25),
U 2 v = [a b 0 ]
a = v& 12 A1¢¢¢A12 u y¢¢2 + 2v& 1v& 2 A1¢¢A12
¢ u y¢¢2 + v& 22 A1¢ A12
¢¢ u y2¢¢
b = v& 12 A1¢¢A12
¢ u y¢¢2 + 2v& 1v& 2 A1¢ A12
¢¢ u y¢¢2 + v& 22 A1A12
¢¢¢ u y2¢¢
where A1¢¢¢ = - A1¢ and A12

¢¢¢ = - A12
¢ .
U 2 v& = éë 2v& 1 A1¢¢A12 u y¢¢2 + 2v& 2 A1¢ A12

¢ u y¢¢2 2v& 1A1¢ A12
¢ u y¢¢2 + 2v& 2 A1A12
¢¢ u y¢¢2 0 ùû
U3 v = [ c d e]
c = v& 12 (v3 + 2) A1¢¢¢A12u y¢¢2 + 2v& 1v& 2 (v3 + 2) A1¢¢A12
¢ u y¢¢2
+ v& 22 (v3 + 2) A1¢ A12
¢¢ u y¢¢2 + 2v& 1v& 3 A1¢¢A12u y¢¢2 + 2v& 2 v& 3 A1¢ A12
¢ u y¢¢2
d = v& 12 (v3 + 2) A1¢¢A12
¢ u y¢¢2 + 2v& 1v& 2 (v3 + 2) A1¢ A12
¢¢ u y¢¢2
¢¢¢ u y¢¢2 + 2v& 1v& 3 A1¢ A12
+ v& 22 (v3 + 2) A1A12 ¢ u y¢¢2 + 2v& 2 v& 3 A1A12
¢¢ u y¢¢2
e = v& 12 A1¢¢A12u y¢¢2 + 2v& 1v& 2 A1¢ A12
¢ u y¢¢2 + v& 22 A1A12
¢¢ u y¢¢2
U3 v& = [f g h]
f = 2v& 1 (v3 + 2) A1¢¢A12u y¢¢2 + 2v& 2 (v 3 + 2) A1¢ A12
¢ u y¢¢2 + 2v& 3 A1¢ A12u y2¢¢
g = 2v& 1 (v3 + 2) A1¢ A12
¢ u y¢¢2 + 2v& 2 (v 3 + 2) A1A12
¢¢ u y¢¢2 + 2v& 3 A1A12
¢ u y2¢¢
h = 2v& 1A1¢ A12u y¢¢2 + 2v& 2 A1A12
¢ u y¢¢2
( V2αˆ ) v = éëa1A1¢¢A12u y¢¢ + a 2 A1¢A12¢ u y¢¢

2 2
a1A1¢ A12
¢ u y¢¢2 + a 2 A1 A12
¢¢ u y¢¢2 0 ùû
( V3αˆ )v = [i j k]
i = a1 (v3 + 2)A1¢¢A12u y¢¢2 + a 2 (v3 + 2) A1¢ A12
¢ u y¢¢2 + a3 A1¢ A12u y2¢¢
j = a1 (v3 + 2)A1¢ A12
¢ u y¢¢2 + a 2 (v3 + 2) A1A12
¢¢ u y¢¢2 + a3 A1A12
¢ u y2¢¢
k = a1A1¢A12u y¢¢2 + a 2 A1A12
¢ u y2¢¢
é u Ty¢¢ A12
T
A1¢T βˆ ù é u Ty2¢¢ A12
T
A1¢¢T β u Ty2¢¢ A12
¢T A1¢Tβ 0 ù
ê 2
ú ê ú
( V βˆ )
T
2
v ê
¢T A1Tβˆ ú = êu Ty¢¢2 A12
= êu Ty¢¢2 A12
ú
¢T A1¢Tβ u Ty¢¢2 A12¢¢T A1Tβ 0 ú
ê ú
ê 0 ú 0 0 0û
ë ûv ë
323
é (v 3 + 2)u Ty¢¢ A12
T
A1¢Tβˆ ù
ê 2
ú
( V βˆ )
T
3
v ê
¢T A1Tβˆ ú
= ê (v3 + 2)u Ty¢¢2 A12
ú
Tˆ
ê u y¢¢2 A12 A1 β
T T
ú
ë û v
é (v3 + 2)uTy2¢¢ A12 T

A1¢¢Tβ (v3 + 2)uTy¢¢2 A12¢T A1¢Tβ uTy¢¢2 A12T
A1¢Tβ ù
ê ú
= ê (v3 + 2)u Ty¢¢2 A12¢T A1¢T β (v 3 + 2)u Ty2¢¢ A12
¢¢T A1T β uTy2¢¢ A12
¢T A1T β ú
ê ú
ë u Ty¢2¢ A12
T
A1¢T β ¢T A1T β
u Ty¢¢2 A12 0 û
From (4.7.16), (4.7.22), and (4.7.23),
X 2 v = éë v& 12 A12
T
A1¢¢Tu z + v& 1v& 2 A12
¢T A1¢T u z v& 12 A12¢T A1¢T u z + v& 1 v& 2 A12
¢¢T A1Tu z 0 ùû
X 2 v& = ëé 2v& 1 A12T
A1¢T u z + v& 2 A12¢T A1T u z v& 2 A12 ¢T A1T u z 0 ùû
( W2 γˆ ) v = éë g1A12T A1¢T u z ¢T A1T u z

g1 A12 0 ùû
éuTz A1 A12δˆ ù éuT A¢ A δ u T A A¢ δ 0 ù

ê ú ê z 1 12 z 1 12
( )
W2T δˆ ê u Tx1¢¢δˆ ú = ê
v
ê ú ê
0 0 0ú
ú
êë 0 úû ë 0 0 0 úû
Due to the complexity of terms required in implicit numerical integration and the
common matrices and trigonometric expressions involved, evaluation of M ( v) and g ( v, v& ) and
their derivatives is carried out in a MATLAB function AMg in Code 4.9.3 of Appendix 4.A.
Logic is imbedded to avoid computation of unnecessary derivatives when explicit numerical
integration is being used and in implicit numerical integration when only equation residual
evaluation is required. Code 4.9.3 supports the same four explicit integration methods and two
implicit methods used in prior examples.
For numerical simulation, the inertia data of Eq. (4.7.26) are used and springs with
coefficients K1 = 100 Nm/rad, K2 = 10,000 Nm/rad, and K3 = 10,000 N/m are applied to the
three degrees of freedom to retain variables near the zero initial condition, v(0) = 0 . An initial
velocity v& (0) = [1 0 0] is imposed and a 10 sec simulation is carried out. Results presented in
T
Fig. 4.9.6 suggest that the rotational degree of freedom v1 is loosely coupled with the other two
degrees of freedom. The variable v1 oscillates with a lower frequency than the other two degrees
of freedom as a near sine wave with only higher frequency perturbation from the other two
degrees of freedom. Total energy for this conservative system was constant to eight places in a
simulation with constant step size h = 0.001 sec, carried out using the Nystrom4 explicit
integrator. Comparable results were obtained using the other five integrators. In simulations with
the explicit RKFN45 and implicit SDIRK54 formulations with variable step size, the code
quickly increased the step size to the upper limit of h = 0.01 sec.
324
Figure 4.9.6 Manipulator Simulation
In use of the manipulator to control motion of an end-effector that is attached at the
outboard end of body 3, torques and force that are determined by a controller,
Q A = [ T1 T2 F3 ] , would be applied to the system by addition to the right side of Eq. (4.9.8).
T
325
Code 4.8 of Appendix 4.A provides six second order ODE numerical solvers for use in
multibody dynamics. Individual integrators presented in Code 4.8 are used in numerous
settings and applications in the remainder of the text.
Numerical integration methods presented in Section 4.8, implemented in Codes 4.9.1 through
4.9.3 of Appendix 4.A to treat the examples of Section 4.7, show excellent numerical
performance. In each example, quantities required for implementation of numerical integration
methods are implemented using Code 4.8 of Appendix 4.A.
While numerical results with examples presented are positive, it is clear that ad-hoc derivation
and numerical solution of the ODE for each example is oppressive, and not conducive to
treatment of general multibody systems. This experience motivates development of broadly
applicable formulations and numerical solution methods that eliminate the need for intricate
and error prone analytical manipulations to obtain and program the equations of mechanical
system dynamics.
326
4.10 Constraint Forces and Differential-Algebraic Equations of Motion
Reaction forces that act on a pair of bodies that are connected by a constraint are derived,
using d’Alembert’s principle (Lanczos, 1962; Pars, 1965) and the Lagrange multiplier theorem
of Section 2.2.2. With expressions for these forces, the variational equations of motion of
multibody systems are written to include all generalized forces that act on the system, yielding
differential-algebraic equations (DAE) of motion.
4.10.1. Generalized Constraint Reaction Forces
For a multibody system that is comprised of nb spatial bodies, generalized coordinates
T
that include position vectors and Euler parameters q i = éëriT p iT ùû for body i are
T
q = éë q 1T L q Tnb ùû for the system. Holonomic kinematic constraints on bodies i and j are
Fk (qi , qj ) = 0, k = 1,L,nh (4.10.1)
and Euler parameter normalization conditions are

Fpi = ( p i Tpi - 1) / 2 = 0, i = 1,...,nb (4.10.2)
where nb is the number of bodies in the system. In vector form, Eqs. (4.10.1) and (4.10.2) are
T
ΦC (q) = éëΦ1T (q) L Φ nh T (q) ùû = 0
T
(4.10.3)
Φ (p) = éëΦp1 (p1 ) L Φp nb (p nb ) ùû = 0
p
Suppressing arguments of constraint functions, for notational convenience, the virtual

displacement equations associated with Eq. (4.10.1) and the Euler parameter normalization
conditions of Eq. (4.10.2) for bodies i and j are
Fqki δqi + Fqkj δq j = 0
(4.10.4)
piT dpi = 0 = pTj dp j
The virtual work of generalized constraint reaction forces Qi k and Q j k that act on
C C
bodies i and j due to constraint k is

δWCk = QiCk Tδqi + QCj k Tδqj (4.10.5)
Since d'Alembert's principle requires that the virtual work of constraint reaction forces is zero for
all kinematically admissible virtual displacements, δW C = 0 for all δqi and δqj that satisfy Eqs.
k
(4.10.4). The Lagrange multiplier theorem of Section 2.2.2 implies existence of a vector l k with
the same dimension as Fk and scalars λ kp and λ kp such that
i j
( )
Q Ci k T δ q i + Q Cj k T δq j + l k T Fqki δ q i + Fqkj δ q j + λ pk i p Ti d p i + λ pk j p Tj dp j = 0 (4.10.6)
327
for arbitrary δqi and δqj . This implies Qi
Ck T
+ lkTFqk + λpk éë0 pTi ùû = 0 and
i i
Q Ck T
j + l F + λ éë 0 p ùû = 0 . Thus,
kT k
qj
k
pj
T
j
T
QiCk = -Fqki Tlk - λkpi éë0 piT ùû
T (4.10.7)
QCj k = -Fqkj Tlk - λpkj éë0 pTj ùû
Deleting the Euler parameter terms and defining q i = [ri fi ] , Eq. (4.10.7) yields generalized
T
constraint forces for planar systems.

4.10.2 Differential-Algebraic Equations of Motion
Summing generalized constraint forces on body i over all constraints that act on body i,
æ ö
QiC º å Q = -å Fqki T l ki - ç å λpkii ÷ éë0 p Ti ùû = -å Fqki T l ki - λpi éë0 piT ùû
Cki T T
ki ki è ki ø ki
æ ö
where λpi º ç å λpkii ÷ . The full vector of constraint generalized forces is thus
è ki ø
Q C = - ΦqCT (q ) λ C - Φ pqT (q ) λ p (4.10.8)
T
where Q C = éëQ1CT L Q Cnb T ùû , λ C = éë λ k1 T L λ k nb T ùû , and λ p = éëλp1 L λpnb ùû . The
T
variational equation of motion for the multibody system of Eq. (4.6.15), including both applied
forces and constraint reaction forces, is
(
dqT M(q)&&
q + ΦCT
q (q)λ + Φq (q)λ - Q (q, q, t) - S(q, q) = 0
C pT p A
& & ) (4.10.9)
where QA (q, q& , t) is the vector of externally applied generalized forces and S (q, q& ) contains
velocity coupling terms, sometimes called Coriolis forces. Combining constraints of Eq. (4.10.3)
for the system as
T
Φ(q ) = éëΦ CT (q ) Φ pT (p ) ùû = 0 (4.10.10)
And associated Lagrange multipliers,

T
λ = éë λ C T λ pT ùû (4.10.11)
Eq. (4.10.9) is
(
dq T M (q)&&
q + ΦqT (q)λ - Q A (q, q& , t) - S(q, q& ) = 0 ) (4.10.12)
Since Eq. (4.10.12) includes all generalized forces that act on the system, all constraints
are accounted for and δq is arbitrary. Its coefficient must therefore be zero, so
&& + ΦqT (q ) λ - Q A (q, q& , t) - S (q, q& ) = 0
M (q )q (4.10.13)
328
These equations and the constraints of Eqs. (4.10.10) comprise a system of differential-algebraic
equations (DAE) of motion for the multibody system. They are called “differential-algebraic”
because (1) they involve derivatives of generalized coordinates and Lagrange multipliers l that
appear algebraically as unknowns and (2) they include the algebraic constraint equations of Eq.
(4.10.10).
The Lagrane multiplier form of constrained equations of motion of Eqs. (4.10.13) and
(4.10.10) have been known since the time of Lagrange (Lagrange, 1788), but were named DAE
only late in the 20th century. These DAE of motion are obtained almost effortlessly, one might
even say cheaply, in this section. As in many endeavors, the old saying “you get what you pay
for” is operative. While an intense research effort has been focused on the DAE of dynamics
during the past 1/2 century, their theoretical properties and numerical methods for their solution
significantly lag that for ODE. These and related issues are addressed in Chapter 7.
It is important to be clear that the DAE of Eqs. (4.10.13) and (4.10.10) implicitly
incorporates the velocity and acceleration equations that are implied by the configuration
constraints of Eq. (4.10.10). The Full system DAE of dynamics, called the Full DAE, is thus the
equation of motion of Eq. (4.10.13) and all three forms of constraint equations,
Φ(q ) = 0
Φ q (q)q& = 0 (4.10.14)
Φ q (q)&& ( )
q = - Φ q (q)q&ˆ q& º - γ(q, q)
&
q
This definition is important, in contrast to formulations called index 3, index2, and index1 that
enforce only the first, second, or third of Eqs (4.10.14), the remaining pair of constraints in each
case being ignored. As might be expected, numerical performance of these formulations are
shown in Chapter 7 to differ radically.
4.10.3. Joint Constraint Reaction Force and Torque
The virtual work of constraint reaction force and torque on body i due to joint k, acting
at the origin of the x¢¢-y¢¢-z¢¢ joint definition frame shown in Fig. 4.10.1, is
i i i
δWik = δr¢¢i PTFi¢¢k + δp¢¢i TTi¢¢k (4.10.15)
where δπ¢¢i is the virtual rotation of the x¢¢-y¢¢-z¢¢ frame, represented in that frame.
i i i
329
Figure 4.10.1. Constraint Reaction Force and Torque on Spatial Body i
The identity δpi = (1 / 2)E (pi )δpi of Eq. (2.6.69) holds, since p δp = 0 is included in the
T T
definition of kinematically admissible virtual displacements of Eq. (4.10.4). The relation

δri = A ( p i )C ik δri¢¢P - A ( p i ) s% ¢i k C ik δπ i ¢¢ is from Eq. (2.4.67). Finally, from Eq. (2.6.4),
Ai = A(pi ) = E(pi )G (pi ) and, from Eq. (2.6.9), E (pi )E(pi ) = I - pp , since the Euler parameter
T T T
normalization condition on pi is included in the system constraints of Eq. (4.10.2). Using these
identities, the virtual work of the generalized constraint force of Eq. (4.10.7) on body i is
T é FrkT lk ù
δW = -δq F l - λ δq éë 0 p ùû = - éë δr
k T kT
qi
k k
pi
T T T
δp ùû ê k T k
T i
ú
êë Fpi l + λpi p i úû
i i i i i i k T
(
= - δriT FrkT
i
)
l k + δp Ti Fpki T l k = -δriT Frki T l k - (1/ 2)δp Ti E(p i )Fpki T l k (4.10.16)
(
= -δr¢¢i PT Cik T A iT Frki T l k - δp¢¢i T (1 / 2)CikT G (p i )Fpki T l k - CiT s%¢i k A iT FrkT
i
lk )
where the identity δpTipi = 0 eliminates l pki from the equation.
Since dWik of Eqs. (4.10.15) and (4.10.16) must be equal, for arbitrary δri¢¢ and δp¢¢i ,
P
é Fi¢¢k ù é - C ik T A iT Φ rki T λ k ù
ê k ú = ê kT ú (4.10.17)
(
ë Ti¢¢ û ëê - C i (1 / 2)G (p i )Φ p i - s%¢i A i Φ ri λ úû
k T k T kT k
)
These joint constraint reaction forces and torques, represented in the x¢i -y¢i -z¢¢i joint definition
frame, are shown in Fig. 4.10.1. In the x¢i -y¢i -z¢i frame, Eq. (4.10.17) reduces to
é Fi¢k ù é - A Ti Φrki T λ k ù
ê kú = ê ú (4.10.18)
(
ë Ti¢ û ëê - (1/ 2)G (pi )Φpi - s%¢i A i Φ ri λ ûú
k T k T kT k
)
330
It is important to note that, whereas Lagrange multipliers associated with Euler parameter
normalization conditions appear in the generalized constraint reaction forces of Eq. (4.10.7) and
in the equations of motion of Eq. (4.10.9), they do not appear in expressions for the joint
constraint reaction force and torque of Eqs. (4.10.17) and (4.10.18).
4.10.4 Evaluation of Constraint Reaction Forces
Even if Eq. (4.10.13) is not used to solve the equations of motion of multibody dynamics,
it may be used with a solution that is obtained by other means to evaluate constraint reaction
forces that are needed for design of joints and components of multibody systems. With a solution
q(t), Eq. (4.10.13) may be written in the form
ΦqT (q)λ = Q A (q,q,
& t) + S(q,q& ) - M(q)q
&& (4.10.19)
Since the matrix Φ q (q) has full row rank, Φ Tq (q) has full column rank and the product
Φ q (q)Φ Tq (q) is nonsingular. Multiplication on the left of Eq. (4.10.19) by Φ q (q) yields
(
Φq (q)ΦqT (q)λ = Φ q (q) Q A (q, q,
& t) + S(q,q& ) - M(q)q
&& ) (4.10.20)
Thus, Eq. (4.10.20) may be solved for all Lagrange multipliers and Eq. (4.10.17) may be used to
evaluate constraint reaction forces that act on each body in the system.
Thus far in the text, the temptation to invoke the classical Lagrange multiplier form of
multibody equations of motion has been resisted, for two reasons. First, it is not needed to
obtain governing equations of motion of multibody systems. Second, it leads to DAE that are at
best a complicated form of the equations of motion of mechanical system dynamics and, as
shown in Chapter 7, are extraordinarily difficult to solve.
On the other side of the ledger, Lagrange multipliers introduced herein, using d’Alembert’s
principal and the Lagrange multiplier theorem of linear algebra, yield explicit expressions for
constraint reaction forces that are important in design of bearings and components of
mechanical systems. The derivation presented in this section yields computable expressions for
constraint reaction forces and the Lagrange multiplier form of the equations of motion that
enforce all three forms of the constraint equations, called the Full DAE.
Key Formulas
&& + ΦqT (q ) λ - Q A (q, q& , t) - S (q, q& ) = 0
M (q )q (4.10.13)
é Fi¢¢k ù é - C ik T A iT Φ rki T λ k ù
ê kú = ê ú (4.10.17)
(
ë Ti¢¢ û êë - C i (1 / 2)G (p i )Φ p i - s%¢i A i Φ ri λ úû
kT k T k T kT k
)
Φ(q ) = 0
Φ q (q)q& = 0 (4.10.14)
Φ q (q)&& ( )
q = - Φ q (q)q&ˆ q& º - γ(q, q)
q
&
331
Appendix 4.A ODE Solution Code
4.A.1 Code 4.8 Second Order ODE Solvers
Components of Code 4.8 that interface with the user are presented in Section 4.A.1.1,
4.A.1.2.
4.A.1.1 User Components of Code 4.8
The initial segment of Code 4.8 involves integration and error control parameters that
underlie the numerical integration methods presented in Fig. 4.A.1.1. Lines 20 through 27 define
parameters that control error in the numerical integration methods supported. Relatively tight
error control parameters are used as defaults, which may be modified to seek greater computer
efficiency or to more tightly constrain error. The initial time step is defined in line 23, followed
by the maximum time step allowed in variable time step integrators in line 24. The final
simulation time is defined in line 27. The integration option to be used is selected in line 34 from
among six integrators shown in lines 30 through 33. Problem data are entered in lines 43 through
48. Solution data storage arrays are defined in lines 51 through 53. Finally, initial conditions are
entered in lines 56 and 57.
1 %Code 4.8: Solution of Second Order ODE M(v)vdd=g(v,vd,t) of Eq.(4.8.43)

2 %on time interval [0,tf]
3
4 %Functions beginning with A require user input; Functions beginning with
5 %B, E, I, and O do not require user input
6
7 %Function g=Agf(t,v,vd,par) defines g(v,vd,t); required for all integrators
8
9 %Function M=AM(v,par) defines M(v); required for all integrators
10
11 %Function [gsv,gsvd] = Agfsvvd(t,v,vd,par) defines partial derivatives of
12 %g(v,vd,t) w.r.t. v and vd; required for implicit integrators
13
14 %Function M2=AM2(v,mu,par) defines derivative of (M(v)mu) w.r.t. v;
15 %required for implicit integrators
16
17 %Function [,,,,]=AdatPart(dat) defines problem dependent data; may be
18 %defined if user wishes to employ named problem data in subroutines
19
20 intol=10^-6; %Tolerance in solving discretized equations
22
23 h=0.001; %Step size
24 hmax=0.01; %Maximum Allowable Step size
25 hvar=2; %hvar=1, variable step;hvar=2, fixed step
26
27 tfinal=12; %Final time
28
29 %Explicit Integration Methods:
30 %Integ=1, Nystrom4; Integ=2, RungeKutta4;
31 %Integ=3, Kutta3/8; integ=4, RKFN
332
32 %Implicit Integration Methods:
33 %Integ=5, Trapezoidal; Integ=6, SDIRK54
34 Integ=6; %Select from among Numerical Integration Options
35
36 nv=[]; %User define variable dimension
37
38 %Fixed Parameter Data List-Partitioning in function BparPart if
40 par=[nv;intol;Atol;hmax;hvar];
41
42 % Problem Data
43 %enter problem data by "name=value;" and enter into dat list.
44 %Problem Parameter Data List-Partitioning in function AdatPart if
46 dat=[;;;;]; %User input data parameters here and in function AdatPart,
47 %as in function BparPart
48 %Example: dat=[m1,J1,k1]
49
51 V=zeros(nv,10);
52 Vd=zeros(nv,10);
53 Vdd=zeros(nv,10);
54
55 % Initial Conditions
56 v0=[]; %User define initial position
57 vd0=[]; %User define initial velocity
58
59 V(:,1)=v0;
60 Vd(:,1)=vd0;
61
62 % NO USER CHANGES/INPUT REQUIRED BEYOND THIS POINT
Figure 4.A.1.1 User Input to Integration Code
4.A.1.2 Computational Components of Code
Computational flow in the main program beyond line 62, which requires no input from
the user, is outlined in Fig. 4.A.1.2. Data for integration are initialized in lines 64 through 81.
Code that implements the six numerical integration methods supported is provided in lines 83
through 120. Integration results are evaluated and recorded in lines 122 through 143.
64 %Integration Preparation
65 n=1; %Time step counter
66 t(1)=0; %Initial time
67
68 %Initial Parameters
69 nch=1;
70
71 % Integration
73 %Time Step Update
74 n=n+1;
75 t(n)=t(n-1)+h;
76 tn=t(n);
333
77
78 % Integration
79 tnm=t(n-1);
80 vnm=V(:,n-1);
81 vdnm=Vd(:,n-1);
82
83 if Integ<5 %Explicit integrators
84 if Integ==1
85 [vn,vdn,vddn,Mcond]=ExplicitNystrom4(tnm,vnm,vdnm,h,par,dat);
86 end
87
88 if Integ==2
89 [vn,vdn,vddn,Mcond]=ExplicitRungeKutta4(tnm,vnm,vdnm,h,par,dat);
90 end
91
92 if Integ==3
93 [vn,vdn,vddn,Mcond]=ExplicitKutta38(tnm,vnm,vdnm,h,par,dat);
94 end
95
96 if Integ==4
97 [vn,vdn,vddn,Mcond,h,nch]=ExplicitRKFN45(n,tnm,vnm,vdnm,h,nch,...
98 par,dat,hvar,nv,Atol,hmax);
99 hrpt(n)=h;
100 end
101
102 Mcondrpt(n)=Mcond; %Record condition number of mass matrix
103 end
104
105 if Integ >4 %Implicit integrators
106
107 if Integ==5
108 [vn,vdn,vddn,jiter,R1Norm,JCond]=ImplicitTrap(n,tn,...
109 V,Vd,Vdd,intol,par,dat,h);
110 jiterrpt(n)=jiter;
111 end
112
113 if Integ==6
114 [vn,vdn,vddn,Maxjiter,R1Norm,JCond,h,err]=ImplicitSDIRK54(n,tn,...
115 V,Vd,Vdd,par,dat,intol,Atol,nv,h,hmax,nch,hvar);
116 hrpt(n)=h;
117 R1Normrpt(n)=R1Norm;
118 errrpt(n)=err;
119 jiterrpt(n)=Maxjiter;
120 end
121
122 JCondrpt(n)=JCond; %Record Jacobian Condition Nujmber
123
124 end
125
126 %Evaluate and Record Solution
127 V(:,n)=vn;
128 vnorm(n)=norm(vn);
129 Vd(:,n)=vdn;
334
130 vdnorm(n)=norm(vdn);
131 Vdd(:,n)=vddn;
132 vddnorm(n)=norm(vddn);
133
134 %Report key data
135 v1(n)=vn(1);
136 v2(n)=vn(2);
137 s1=sin(vn(1));
138 s2=sin(vn(2));
139 c1=cos(vn(1));
140 c2=cos(vn(2));
141 TE(n)=0.5*vdn'*AM(vn,par,dat)*vdn+9.8*(2*s1+s2); %Total Energy
142
143 end
Figure 4.A.1.2 Main Code Computational Flow
Computing functions that underlie the main code are identified in Fig. 4.A.1.3.
Computing functions include AdatPart and BparPart that partition data in the dat and par arrays,
for use in all computing functions. The user enters g(t, v, v& ) in Function Agf ,
g v (t, v, v& ) and g v& (t, v, v& ) in Function Agfsvvd, M( v ) in Function AM, and ( M( v )mˆ ) v in
Function AM2. The six integration Functions listed implement the indicated numerical
integration method. Finally, the ODEfunct Function computes the solution &v& of Eq.
Error! Reference source not found.. Internal details of the Computing Functions listed in Fig.
4.A.1.3 are not presented here, since each of the functions is documented internally and the user
need not modify these functions in applications.
AdatPart
Agf
Agfsvvd
AM
AM2
BparPart
ExplicitKutta38
ExplicitNystrom4
ExplicitRKFN45
ExplicitRungeKutta4
ImplicitSDIRK54
ImplicitTrap
ODEfunct
Figure 4.A.1.3 Computing Functions in Code 4.8
4.A.2 Code 4.A.1 Planar Double Pendulum
See MATLAB Code in accompanying software file.
4.A.3 Code 4.A.2 Planar Slider-Crank
4.A.4 Code 4.A.3 Three Degree of Freedom Robotic Manipulator
335
336
337
CHAPTER 5
Tangent Space ODE for Holonomic Systems
5.0 Introduction
As the reality that DAE formulations of mechanical system dynamics introduced in
Sections 4.10.2 and 4.10.5 are very difficult to solve has matured (Petzold, 1982; Haug, 1989;
Ascher and Petzold, 1998; Bauchau and Laulusa, 2008), the search for ODE formulations has
intensified. Notably, Maggi (1896; 1901) and Kane (1985) formulations that are valid for
nonholonomic systems have been used for holonomic systems by enforcing velocity constraint
equations and leaving the configuration constraint equations unenforced, except at the initial
time (Tseng, Ma, and Hulbert, 2003; Bauchau and Laulusa, 2008; Laulusa and Bauchau, 2008;
Garcia de Jalon, Callejo, and Hidalgo, 2012). Problems with this formulation are summarized in
Appendix 5.D.
Extending the Maggi/Kane approach (Haug, 2018d), parameterizations of system
kinematics in terms of holonomic constraint tangent space generalized coordinates are presented
in this chapter to reduce the equations of motion for planar and spatial systems to ODE, without
relying on the use of Lagrange multipliers. ODE solution methods of Section 4.8 are applied to
obtain simulation results that satisfy configuration, velocity, and acceleration constraint
equations.
A particle on the unit sphere and a spinning top with Euler parameter orientation
coordinates are used as model problems in Section 5.1, for which globally valid independent
generalized coordinates do not exist. The tangent space parameterization method, based on
differentiable manifold theory of Sections 3.5 and 3.6, is used to reduce variational equations of
motion to ODE. Numerical results obtained using Runge-Kutta integrators verify that
configuration, velocity, and acceleration constraints are satisfied and that no singularities are
encountered. A broadly applicable tangent space parameterization of multibody system
kinematics and dynamics is presented in Section 5.2 that enforces constraints at configuration,
velocity, and acceleration levels. An algorithm for computation of constraint reaction forces
using ODE solutions is presented.
ODE initial-value problems of multibody system dynamics are solved in Section 5.3,
using tangent space generalized coordinates. Computational algorithms for implementing the
approach with explicit and implicit numerical integrators of Section 4.8 are presented and used in
solution of examples in Section 5.4. Performance of the method in satisfying all three forms of
kinematic constraint, based on error tolerances in the formulation, is verified.
An Index 0 DAE formulation of the equations of multibody dynamics is derived in
Section 5.5, using tangent space generalized coordinates. The tangent space ODE formulation is
modified to include Lagrange multipliers that are introduced in Section 4.10 to represent
constraint reaction forces. This makes the Index 0 DAE formulation ideally suited for simulation
of systems with friction, as is done in Chapter 8. Equivalence of the ODE and Index 0 DAE
formulations is shown. Explicit and implicit numerical integration algorithms for numerical
integration of the Index 0 DAE formulation are presented in Section 5.5 and used for numerical
solution of examples in Section 5.6.
338
Rudimentary general-purpose MATLAB computer Codes 5.7 and 5.9 of Appendix 5A
are presented in Sections 5.7 and 5.9 for modeling and simulation of planar and spatial systems,
respectively. Components of the codes that enable user definition of models and simulation
alternatives are presented, along with an outline of computational components of the codes.
Dynamic simulations of planar and spatial systems are presented in Sections 5.8 and 5.10, using
Codes 5.7 and 5.9 of Appendix 5.A, including larger scale system simulations that would be
intractable if pursued in an ad-hoc fashion.
Theorems that show the variational, tangent space ODE, Index 0 DAE, and Full DAE
formulations of holonomic multibody dynamics are equivalent and well posed are proved in
Section 5.11.
Derivatives of planar and spatial constraints of Sections 3.2 and 3.3 and kinetic
derivatives that are required to implement ODE and Index 0 DAE formulations are presented in
Appendix 5.B, followed by a summary of key formulas for Chapter 5 in Appendix 5.C. Finally,
deficiencies of the Maggi and Kane equations, when used to represent holonomic systems, are
presented in Appendix 5.D. Their extension for treatment of holonomic systems, using the
tangent space ODE formulation of this chapter has been outlined in (Haug, 2018d).
339
5.1 Model Problems That Require Local Coordinates on a Differentiable Manifold
As shown in Section 3.5.1, even for a single particle on the two-dimensional surface of
the unit sphere, it is not possible to find two generalized coordinates that parameterize the entire
surface without encountering a singularity. Similarly, it is observed in Section 2.5.3 that it is not
possible to parameterize the entire three-dimensional space of orientations of a rigid body with
three orientation generalized coordinates. Examples presented in Section 3.7 demonstrate that
singularities may arise in the equations of system kinematics that preclude definition of globally
valid independent generalized coordinates. Finally, it is shown in Chapter 3 that multibody
system kinematic models lead to high dimensional nonlinear equations that defy practical
definition of globally valid independent generalized coordinates, even if they exist. Thus, local
generalized coordinates in the differential geometric setting outlined in Sections 3.5 and 3.6 are
required to obtain equations of motion that are valid everywhere in the regular configuration
space. Two model problems are treated in this section, using the tangent space parameterization
introduced in Section 3.5, that resolve the difficulties noted and motivate a broadly applicable
approach to multibody system dynamics that is developed in Section 6.5.2.
It is critical in formulation of equations of motion as ODE that independent generalized
coordinates be defined that satisfy configuration, velocity, and acceleration constraints, called
Lagrangian coordinates in the classical literature (Pars, 1965). Otherwise, the equations of
motion obtained will be in error. Tangent space local parameterizations of the constraint
manifold, following the approach in Section 3.5.2, are presented for these model problems,
leading to locally defined independent generalized coordinates that satisfy configuration,
velocity, and acceleration constraint equations. Use of these coordinates and the variational
equations of motion derived in Chapter 4 lead to ODE of motion that are ideally suited for digital
computer implementation. The approach illustrated for these model problems is generalized in
the remainder of the chapter, as a basis for computational implementation for ODE simulation of
broad classes of multibody dynamic systems.
5.1.1 Particle on Unit Sphere
Consider the particle on a unit sphere shown in Fig. 5.1.1 that was introduced in
Example 1.2.1 of Section 1.2. Tangent space parameterization of the constraint manifold is
presented in Example 3.5.1 of Section 3.5.1, summarized as follows.
5.1.1.1 Tangent Space Generalized Coordinates
Vectors q to points on the unit sphere must satisfy the holonomic kinematic constraint,
Φ(q) = (qTq - 1) / 2 = 0 (5.1.1)
340
At a point q 0 on the unit sphere, the vector q 0 is normal to the surface of the sphere and
vectors w tangent to the surface satisfy F q (q 0 )w = q0T w = 0 . Since q 0 cannot coincide with all
three of the unit coordinate vectors u x , u y , and u z , say q0 ¹ aux , define the unit vector
( )
V1 = 1 / u% x q 0 u% x q 0 that is orthogonal to u x and q0 . Next, define the unit vector
V2 = (1 / q% V ) q% V that is orthogonal to q
0 1 0 1 0
and V1 . Since the unit vectors V1 and V 2 are
orthogonal and both are orthogonal to q 0 , they form a basis for the tangent plane, or tangent
space, to the sphere at point q 0 . Define
U = ΦTq (q0 ) = q0 (5.1.2)
and
V = éë V1 V 2 ùû (5.1.3)
where V T V = I, V T U = 0, and U T U = 1 . Since the unit vector U is orthogonal to the two linearly
independent columns of V, the columns of V and U form a basis for R 3 .
Every vector q in R 3 can be uniquely written in terms of tangent space generalized
coordinates v Î R 2 and u Î R1 as
q = q0 + Vv - uU (5.1.4)
where the sign of the third term on the right is selected to represent a projection onto the regular
configuration space from the tangent space, as shown schematically in Fig. 5.1.2. Multiplying
Eq. (5.1.4) on the left by V T and UT and using the relations U T V = I and V T U = 0 yields the
inverse relations
v = V T (q - q0 )
(5.1.5)
u = - U T (q - q 0 )
In particular, v(t 0 ) = v 0 = 0 and u(t 0 ) = u 0 = 0 .
341
Figure 5.1.2 Projection onto Regular Configuration Space
Requiring that q of Eq. (5.1.4) satisfies the holonomic constraint of Eq. (5.1.1) yields
q q - 1 = u 2 - 2u + v T v = 0 . Using the quadratic formula, u = 1 ± 1 - v T v . Clearly, this
T
parameterization is valid only if v T v < 1 . Geometrically, this means that if v T v > 1 , the line
perpendicular to the tangent space through the point q0 + Vv does not intersect the unit sphere in
R 3 . Since u = 0 if v = 0 , the minus sign must be selected, so
u = 1 - 1 - vT v º h( v) (5.1.6)
and Eq. (5.1.4) is
( )
q º ψ(v) = q0 + Vv - 1 - 1 - v T v q0 = Vv + 1 - v T vq0 (5.1.7)
The vector q of Eq. (5.1.7) is shown in Section 3.5.1 to satisfy Eq. (5.1.1), for all v in a
neighborhood of v 0 = 0 .
Since V and q 0 are constant, differentiation of Eq. (5.1.4) yields
q& = Vv& - u& q 0

(5.1.8)
&& = Vv
q && - &&
uq 0
Multiplying Eqs. (5.1.8) on the left by V T and q0T and manipulating yields the inverse relations
v& = V Tq&
u& = -q0Tq&
(5.1.9)
v = V T&&
&& q
u = -q0T&&
&& q
In particular, the first of Eqs. (5.1.9) implies v& 0 = V Tq& 0 .
Substituting q& of Eq. (5.1.8) into the kinematic velocity constraint Φq q& = qTq& = 0 yields
the relation q T Vv& - u& q Tq0 = 0 . Since q 0T q 0 = 1 , q Tq 0 ¹ 0 in a neighborhood of q 0 and
æ 1 ö
u& = ç T 0 ÷ qT Vv& (5.1.10)
èq q ø
in that neighborhood. Substituting this relation into the first of Eqs. (5.1.8),
342
æ 1 ö é æ 1 ö ù
q& = Vv& - ç T 0 ÷ q 0r T Vv& = ê I - ç T 0 ÷ q0q T ú Vv& = D(q )v& (5.1.11)
èq q ø ë èq q ø û
where
é æ 1 ö ù
D(q) º ê I - ç T 0 ÷ q 0q T ú V (5.1.12)
ë èq q ø û
Substituting q of Eq. (5.1.7) and q& of Eq. (5.1.11) into the kinematic velocity constraint verifies
that it is satisfied for arbitrary v in a neighborhood of v 0 and for all v& . A result identical in form
to Eq. (5.1.11) holds for differentials of variables; i.e.,
dq = D( q )dv (5.1.13)
Substituting from the second of Eqs. (5.1.8) into the kinematic acceleration constraint
&& = q Tq
Φq ( q )q && - &&
&& + q& Tq& = 0 yields the relation q T Vv uqTq0 + q& Tq& = 0 . In a neighborhood of q 0 ,
æ 1 ö
u = ç T 0 ÷ ( q T Vv
&& && + q& Tq& ) (5.1.14)
èq q ø
Substituting this relation into the second of Eqs. (5.1.8) and manipulating,
æ q& Tq& ö
q = D(q )&&
&& v - ç T 0 ÷ q0 (5.1.15)
èq q ø
Substituting q of Eq. (5.1.7), q& of Eq. (5.1.11), and q && of Eq. (5.1.15) into the kinematic
acceleration constraint verifies that it is satisfied for arbitrary v in a neighborhood of v 0 and for
all v& and &&
v . Equations (5.1.7), (5.1.11), and (5.1.15) thus yield position, velocity, and
acceleration that satisfy all three forms of kinematic constraint.
5.1.1.2 ODE of Motion
With gravitational acceleration g acting in the negative z direction on the particle of Fig.
5.1.1, the variational equation of motion of Eq. (4.1.25) is
dq T [ mq
&& + mgu z ] = 0 (5.1.16)
which must hold for all δq such that Fq (q)δq = 0 . Substituting δq and q
&& from Eqs. (5.1.13)
and (5.1.15) into Eq. (5.1.16),
é æ q& Tq& ö ù
dv T D(q)T ê mD(q )&&
v - m ç T 0 ÷ q0 + mgu z ú = 0 (5.1.17)
ë èq q ø û
é q Tq 0 ù
Since Fq (q)δq = qT D(q )δv = êq T - T 0 q T ú Vδv = 0 , Eq. (5.1.17) must hold for arbitrary δv ,
ë q q û
æ q& Tq& ö
v = mDT (q) ç T ÷ U - mgDT (q)uz
mDT (q)D(q)&& (5.1.18)
èq Uø
343
which, with Eqs. (5.1.7) and (5.1.11) is a second order ODE in v. At q = q 0 , q 0T V = 0 and Eq.
(5.1.12) is D(q0 ) = V . Since VT V = I , the reduced mass matrix mD T ( q ) D( q ) on the left of Eq.
(5.1.18) at q 0 equals mI, which is positive definite, hence nonsingular. Since D(q ) is a
continuous function of q, in a neighborhood of q 0 the symmetric matrix mD T ( q ) D( q ) is positive
definite, hence nonsingular.
The first of Eqs. (5.1.5) and Eq. (5.1.9) yield initial conditions
v0 = 0
(5.1.19)
v& 0 = V Tq& 0
Finally, Eq. (5.1.18) is a second order ODE of the form of Eq. (4.8.45),
v = g( v,v& , t)
M( v)&& (5.1.20)
where, from Eqs. (5.1.7) and (5.1.11), q = q(v) = Vv + 1 - vT vq0 and

é æ 1 ö 0 T ù
q& = q& ( v, v& ) = D(q( v ))v& = ê I - ç T 0 ÷
q q ( v )ú Vv& , so
ë è q ( v )q ø û
M ( v ) = m D T ( q( v ) ) D ( q( v ) )
æ q& T ( v , v& )q& ( v, v& ) ö 0 (5.1.21)
g(t, v, v& ) = mDT (q( v )) ç ÷ q - mgD (q( v ))u z
T
è q ( v )q
T 0
ø
The arguments of all functions of q and q& involved are shown, to emphasize that they are
functions of v and v& . Since M(v) is nonsingular in a neighborhood of v0 , the initial-value
problem of Eqs. (5.1.20) and (5.1.19) has a unique solution in this neighborhood of v 0 . If Eq.
(5.1.20), with initial conditions of Eq. (5.1.19), is numerically integrated for v and its derivatives,
using an ODE solver, q, q& , and q&& determined by Eqs. (5.1.7), (5.1.11), and (5.1.15) satisfy all
three forms of kinematic constraint to within the accuracy of the integrator.
Since v is only a valid parameterization of the constraint manifold for v T v < 1 , bounds on
the magnitude of v must be monitored during numerical integration. Further, the condition
number, defined in Section 2.2.8, of M ( v ) in Eq. (5.1.21) must be bounded to avoid singularity
in Eq. (5.1.18). If an assigned tolerance on the norm of v or condition number of M ( v ) is
exceeded, the current value of q is designated q 0 , V is redefined, and the integration process is
continued with new initial conditions on v and v& , namely from Eqs. (5.1.19) , v 0 = 0 and
v& 0 = V Tq& 0 . The process is continued to a specified final time, with results reported in terms of q
and its derivatives, using Eqs. (5.1.7), (5.1.11), and (5.1.15). The user of the process need not
know values of v and its derivatives that have been computed. This process is guaranteed to be
continued, as shown in Fig. 3.7.4, since with q Tq = 1 , Φq (q) = qT never fails to have full rank.
344
5.1.1.3 Numerical Solution
Inertia properties used for simulation are m = 1 kg and g = 9.8 m/sec2 . The initial
configuration is with the mass at the pole on the z axis, q 0 = u z = [0 0 1] , and initial velocity
T
is q& 0 = u y = [0 1 0] . This begins motion at the singularity in the q - f generalized

T
coordinates of Example 1.2.1 and necessarily passes through the singular point q = u y for the
a - b generalized coordinates of the same example. As a check on conservation of energy, total
energy is TE = (1 / 2)mq& T q& + mgq z , where qz is the z component of the vector q. MATLAB
Code 4.8 of Appendix 4.A was adapted for solution of this initial-value problem as Code 5.1.1 of
Appendix 5.A. Four explicit numerical integration algorithms of Section 4.8 were implemented.
Plots in Fig. 5.1.3 show the y and z coordinates of the particle as time progresses over a
10 sec interval. As required, the x coordinate of the particle remained zero and total energy was
constant for this conservative system, both within computer precision. Numerical results reported
were obtained using the Nystrom4 explicit integrator of Table 4.8.8, with constant step size
0.001 sec. Comparable results were obtained with three other explicit integrators. Further, all
three forms of kinematic constraint were satisfied with maximum error equal to computer
precision. Over the 10 sec simulation, with 10,000 time steps, 36 reparameterizations were
required (278 time steps per reparameterization), all due to a bound of 0.75 on the norm of v. No
singularities were encountered, even though the simulation passed through the y and z poles
several times.
Figure 5.1.3 y and z Coordinates of Particle on Unit Sphere

5.1.2 Symmetric Top
Consider the symmetric Top shown in Fig. 5.1.4. A body-fixed x ¢-y¢-z¢ reference frame
has its origin at the tip, with the z¢ axis as the axis of symmetry for the Top. The tip is attached
to the origin of the inertial x-y-z reference frame so r = 0 and orientation of the top is defined by
Euler parameters, which are the generalized coordinates of the system. Admissible Euler
345
T
parameter vectors p = éë e 0 eT ùû Î R 4 must satisfy the normalization condition of Eq. (2.5.25)
and its derivatives,
Φ(p ) = ( p Tp - 1) / 2 = 0
Φp (p )p& = p Tp& = 0 (5.1.22)
&& = p Tp
Φp (p )p && = -p& Tp&
The conditions of Eq. (5.1.22) are orientation, velocity, and acceleration constraints.
Figure 5.1.4 Symmetric Top with Tip Fixed

5.1.2.1 Tangent Space Generalized Coordinates
Let Euler parameters at the initial time t 0 , p(t0 ) = p0 , satisfy the first of Eqs. (5.1.22). The
% = {p : pTp = 1 and Φ (p) = pT ¹ 0} at p0 in
tangent space for the regular configuration space C p
R 4 is the set of all vectors p& such that p0T p& = 0 .

As in Section 3.5, define
U = Φp (p0 )T = p0 (5.1.23)
Equations (2.6.6) and (2.6.8) imply G(p 0 )p0 = 0 and G(p0 )G(p0 )T = I , so columns of G(p0 )T lie
in the tangent space of the regular configuration space at t 0 and are orthogonal unit vectors.
Thus, they form a basis for the tangent space, denoted
V = G(p 0 )T (5.1.24)
where V T V = I and VT U = VTp0 = 0 . Since the unit vector U is orthogonal to the three linearly
independent columns of V, the columns of V and U form a basis for R 4 . Thus, every vector p in
R 4 can be uniquely written in terms of tangent space coordinates v and u as
p = p0 + Vv - uU (5.1.25)
346
where the sign of the third term on the right is selected to represent a projection onto the
constraint manifold from the tangent space, as shown schematically in Fig. 5.1.5.
Figure 5.1.5. Orthogonal Projection onto Constraint Manifold

Multiplying Eq. (5.1.25) on the left by V T and UT and using the relations V T V = I ,
VT U = 0 , and U T U = 1 yields the inverse relations
v = V T (p - p 0 )
(5.1.26)
u = -U T (p - p 0 )
In particular, Eq. (5.1.26) implies v(t 0 ) = v 0 = 0 and u(t 0 ) = u 0 = 0 .

Requiring that p of Eq. (5.1.25) satisfies the first of Eqs. (5.1.22) yields
p Tp - 1 = u 2 - 2u + v T v = 0 (5.1.27)
Using the quadratic formula, u = 1 ± 1 - v T v . Clearly, the parameterization of Eq. (5.1.25) is

only valid if v T v < 1 . Geometrically, this means that if v T v > 1 , the line perpendicular to the
tangent space through the point p0 + Vv does not intersect the unit sphere in R 4 . Since u = 0 if
v = 0 , the minus sign must be selected, so
u = 1 - 1 - vT v (5.1.28)
Equation (5.1.25) is thus
(
p = p0 + Vv - 1 - 1 - v T v U ) (5.1.29)
Substituting p of Eq. (5.1.29) into the first of Eqs. (5.1.22) verifies that it is satisfied for arbitrary
v in a neighborhood of v 0 = 0 .
Since V and U are constant, differentiation of Eq. (5.1.25) yields
p& = Vv& - u& U
(5.1.30)
&& = Vv
p && - &&
uU
As in the derivation of the inverse relation of Eq. (5.1.26), multiplying Eqs. (5.1.30) on the left
by V T and U and using the relations V T V = I , VT U = 0 , and U T U = 1 yields the inverse
relations
347
v& = V Tp&
u& = -U Tp&
(5.1.31)
v = V T&&
&& p
u = -U T&&
&& p
Substituting p& of Eq. (5.1.30) into the second of Eqs. (5.1.22) yields the relation
p Vv& - u& p T U = 0 . Since U T U = 1 , pT U ¹ 0 in a neighborhood of p0 and
T
æ 1 ö
u& = ç T ÷ pT Vv& (5.1.32)
èp Uø
in that neighborhood. Substituting this relation into the first of Eqs. (5.1.30),
æ 1 ö é æ 1 ö ù
p& = Vv& - ç T ÷ UpT Vv& = ê I - ç T ÷ Up T ú Vv& = D(p) v& (5.1.33)
èp Uø ë èp Uø û
where
é æ 1 ö ù
D(p) º ê I - ç T ÷ Up T ú V (5.1.34)
ë èp Uø û
Substituting p of Eq. (5.1.29) and p& of Eq. (5.1.33) into the second of Eqs. (5.1.22) verifies that
it is satisfied for all v in a neighborhood of v 0 = 0 and arbitrary v& . A result identical in form to
Eq. (5.1.33) holds for differentials; i.e.,
dp = Ddv (5.1.35)
Substituting from the second of Eqs. (5.1.30) into the third of Eqs. (5.1.22) yields the
&& - &&
relation p T Vv upT U + p& Tp& = 0 . Thus, in a neighborhood of p0 ,
æ 1 ö
u = ç T ÷ ( p T Vv
&& && + p& Tp& ) (5.1.36)
èp Uø
Substituting this relation into the second of Eqs. (5.1.30) and manipulating,
æ p& Tp& ö
p = D(p)&&
&& v -ç T ÷U (5.1.37)
èp Uø
Substituting p of Eq. (5.1.29), p& of Eq. (5.1.33), and p && of Eq. (5.1.37) into the third of
Eqs. (5.1.22) verifies that it is satisfied for all v in a neighborhood of v 0 = 0 and arbitrary
v& and &&
v . Equations (5.1.29), (5.1.33), and (5.1.37) thus yield orientation, velocity, and
acceleration that satisfy all three forms of constraint in Eqs. (5.1.22), as long as v < 1 .
5.1.2.2 ODE of Motion
With gravitational acceleration g acting in the negative z direction on the top of Fig.
5.1.4, n¢ = -mgu% ¢z A ( p ) u z + n¢A , where u¢z and uz are unit vectors along the positive z¢ and z
T
348
axes of the body fixed and inertial reference frames, respectively, and n¢A is an externally
applied torque. Since r = dr = &&
r = 0 , the variational equation of motion of Eq. (4.3.23) is
p - 4G(p& )T J ¢G(p& )p - G(p)T ( - mgu% ¢z A(p)T u z + n¢A )ûù = 0 (5.1.38)
dp T ëé2G(p )T J ¢G(p )&&
which must hold for all δp such that Fp (p)δp = p Tδp = 0 . Substituting δp and p
&& from Eqs.
(5.1.35) and (5.1.37) into Eq. (5.1.38),
é æ æ p& Tp& ö ö ù
ê2G(p ) J ¢G ( p ) ç D(p )&&
v - ç T ÷ U ÷ - 4G(p& )T J ¢G ( p& ) p ú
T
dv T D(p)T ê è èp Uø ø ú=0
ê
(
êë -G(p) - mgu% ¢z A ( p ) u z + n¢
T T A
) ú
úû
é pTU ù
Since p Tδp = p T D(p)δv = êp T - T p T ú Vδv = 0 for aarbitrary δv , this equation must hold for
ë p U û
arbitrary dv . Thus,
æ p& Tp& ö
2D ( p ) G(p )T J ¢G ( p ) D ( p ) &&
v = 2 ç T ÷ D ( p ) G(p )T J ¢G ( p ) U
T T
èp Uø (5.1.39)
T T T
(
+4D ( p ) G(p& ) J ¢G ( p& ) p + D ( p ) G(p) -mgu% ¢z A ( p ) u z + n¢
T T A
)
At p0 , D = V = G ( p0 ) and, since G ( p 0 ) G ( p 0 ) = I , the reduced mass matrix 2DT G T J ¢GD on
T T
the left of Eq. (5.1.39) at p0 equals 2J ¢ , which is positive definite. Thus, in a neighborhood of
p0 , the symmetric matrix 2 D T ( p )G T ( p )J ¢G( p ) D( p ) is positive definite, hence nonsingular. The
first of Eqs. (5.1.26) and (5.1.31) yield initial conditions
v0 = 0
(5.1.40)
v& 0 = V Tp& 0
Equation (5.1.39) is a second order ODE of the form of Eq. (4.8.45),
v = g(t,v,v& )
M( v )&& (5.1.41)
(
where, from Eqs. (5.1.29) and (5.1.33), p(v) = p 0 + Vv - 1 - 1 - v T v p 0 , )
é æ 1 ö 0 ù
p& ( v, v& ) = ê I - ç T 0 ÷
p p( v)T ú Vv& , and
ë è p( v) p ø û
349
M ( v ) = 2D ( p ) G(p )T J ¢G ( p ) D ( p )
T
æ p& Tp& ö
g( v, v& , t) = 2 ç T 0 ÷ D ( p ) G(p)T J ¢G ( p ) p0
T
èp p ø (5.1.42)
+ 4D ( p ) G(p& ) J ¢G ( p& ) p
T T
(
+ D ( p ) G(p)T - mgu% ¢z A ( p ) u z + n¢A
T T
)
The arguments of all functions of p and p& involved are shown, to emphasize that they are
functions of v and v& . If Eq. (5.1.39), with initial conditions of Eq. (5.1.40), is numerically
integrated using an ODE solver, all three forms of constraint of Eqs. (5.1.22) will be satisfied, to
within the accuracy of the integrator.
Since v is only a valid parameterization of the constraint manifold for v T v < 1 , bounds on
the magnitude of v must be monitored during numerical integration. Further, the condition
number, defined in Section 2.2.8, of M ( v ) in Eq. (5.1.42) must be bounded to avoid singularity
in Eq. (5.1.41). If an assigned tolerance on norm of v or condition number of M ( v ) is exceeded,
the current value of p is designated p0 , U = p 0 , and V = G( p 0 )T are redefined. The integration
process is then continued with new initial conditions on v and v& , namely from Eqs. (5.1.26) and
(5.1.31), v 0 = 0 and v& 0 = V T p& 0 . The process is continued to a specified final time, with results
reported in terms of Euler parameters and their derivatives, using Eqs. (5.1.29), (5.1.33), and
(5.1.37). The user of the process need not know values of v and its derivatives that have been
computed. This process can be continued, as shown in Fig. 3.7.4, since with p Tp = 1 , Φp (p) = pT
never fails to have full rank.
5.1.2.3 Numerical Solution
Inertia properties used for simulation are J¢ = diag(120,120,30) kg × m2 , relative to the
noncentroidal body fixed coordinate system at the tip, m = 30 kg, and g = 9.8 m/sec2 . The initial
configuration is with the top vertically upward; i.e., p 0 = [1 0 0 0] . The initial angular
T
velocity in the body fixed reference frame is w¢0 = [ e e omegaz0] , where initial angular
T
velocity components ε = 10-12 about the x ¢ and y¢ axes play the role of perturbations from the
vertical configuration. From Eq. (2.6.64), p& 0 = 0.5G (p 0 )T w¢0 . As a check on conservation of
energy, total energy is TE = 2p& T G( p ) T J ¢G( p )p& + mgs z , where sz is the z component of the vector
s = A(p)u¢z from the tip to the centroid of the top. MATLAB Code 4.8 of Appendix 4.A was
adapted for solution of this initial-value problem as Code 5.1 of Appendix 5.A. Four explicit
numerical integration algorithms of Section 4.8 were implemented.
Plots in Fig. 5.1.6 show the x and y coordinates of the centroid as time progresses over a
100 sec interval, obtained with four initial values of z¢ initial angular velocity omegaz0. As
omegaz0 is increased from 12 rad/sec at the upper left to 13 5 rad/sec at the lower right, the
radius of precession reduces from 0.5 m to 2 ´10-12 m. Thus, stability of the spinning top is
achieved for the higher initial angular velocity.
350
All simulations reported total energy and z¢ momentum constant to eight places for this
conservative system. Numerical results reported were obtained using the Nystrom4 explicit
integrator of Table 4.8.8, with constant step size 0.001 sec. Comparable results were obtained
with three other explicit integrators. During integration, 793 reparameterizations in 100,000 time
steps (130 time steps per reparameterization) were required, all due to a limit of 0.75 on the norm
of v. Computing cost to reparameterize the equations of motion was 0.03% of total CPU time.
The maximum error over the simulation interval in satisfying each of the constraints in
Eqs. (5.1.22) was less than 10-14 . The tangent space equations of motion thus account for
nonlinear effects of Euler's equation of motion and the Euler parameter normalization, velocity,
and acceleration constraints of Eqs. (5.1.22), as predicted in the case of this analytical reduction,
to machine precision.
Figure 5.1.6 x-y Trajectory of Centroid
Adding a torque -Dcf ω¢ = -4Dcf G(p)p& due to aerodynamic drag applied to the
2 2
top about the body-fixed z¢ axis leads to a loss of kinetic energy and an increasing magnitude in
the radius of precession over the simulation interval. With aerodynamic drag coefficient Dcf =
0.005, x-y trajectories and plots of norm of angular velocity (omegnorm) are shown in Fig. 5.1.7,
with initial vertical angular velocities of 14 and 16 rad/sec. For both initial angular velocities,
351
there was a loss in total energy of approximately 35% over the 100 sec simulation. During
integration, 786 reparameterizations in 100,000 time steps (127 time steps per
reparameterization) were required, all due to a limit of 0.75 on the norm of v. Constraint errors
were comparable to those reported for the simulation without damping.
Figure 5.1.7 x-y Trajectory of Centroid and Norm of Angular Velocity with Drag
The computational approach presented for model problems treated in this section (1) employs
independent generalized coordinates that satisfy all three forms of kinematic constraint, (2)
defines initial conditions on independent position and velocity, (3) forms ODE of motion with
no singularities, and (4) integrates the equations of motion using numerical methods to an
accuracy specified by the user. This methodology is based on rigorous foundations of
analytical dynamics, mathematics, numerical analysis, and computational science. These
statements serve as a challenge in subsequent sections to maintain rigor in generalizing the
approach presented in this section to more realistic applications.
Numerical solution methods of Section 4.8 are implemented in Codes 5.1.1 and 5.1.2 of
Appendix 5.A and demonstrate the ability to reliably and accurately solve the second order
ODE of motion.
352
5.2 Tangent Space ODE
Tangent space parameterization of the holonomic constraint manifold presented in
Section 3.5 is extended to the realm of dynamics, including holonomic constraints with explicit
time dependence associated with kinematic drivers that are treated in Sections 3.2.2 and 3.3.5.
Kinematic relations among displacements, virtual displacements, velocities, and accelerations are
used with the variational equations of motion of Section 4.8 to obtain second order ODE of
dynamics and associated initial conditions in independent tangent space generalized coordinates.
5.2.1 Tangent Space Kinematics with Time Dependent Constraints
A time dependent holonomically constrained multibody system is represented by nhc
holonomic constraint equations in ngc generalized coordinates q and time t, of the form
F (q, t ) º F (q) - f (t) = 0 (5.2.1)
where the decoupled dependence on q and t is associated with driving constraints presented in
Sections 3.2 and 3.3. With this form of constraint, the constraint Jacobian and numerous other
derivative expressions do not involve explicit time dependence. Their time derivatives yield
velocity and acceleration constraint equations,
F q (q)q& = f t (t) (5.2.2)
F q (q)q ( )
&& = - F q (q)q&ˆ q& + ftt ( t ) º - γ(q,q,
q
& t) (5.2.3)
which, along with Eq. (5.2.1), comprise three forms of constraint equations that must be
satisfied. The constraint Jacobian Φq (q ) at a configuration q 0 that satisfies Eq. (5.2.1) at t 0 ;
i.e., F(q0 ) - f (t 0 ) = 0 , is evaluated as F q (q0 , t 0 ) = Φq (q0 ) . With time held fixed, Eq. (5.2.2)
yields F q (q0 )dq = 0 , defining kinematically admissible virtual displacements dq that are tangent
to the regular configuration space,
C(t) {
% = q : Φ(q) - f (t) = 0 and rank ( Φ (q ) ) = nhc
q } (5.2.4)
The fact that the constraint Jacobian Fq (q ) does not depend explicitly on time in this
formulation minimizes the complexity of system kinematics.
The constraint Jacobian F q (q0 ) defines a matrix U whose column vectors are orthogonal
to the tangent plane of the constraint manifold at q 0 ; i.e.,
U º F0T
q = Fq (q )
0 T
(5.2.5)
For a constraint Jacobian with full rank, the columns of U are linearly independent and U T U is
positive definite, hence nonsingular. A second matrix V is defined as the solution of
Φ 0q (q0 )V = U T V = 0
(5.2.6)
VTV = I
353
using singular value decomposition (Strang, 1980). The second of Eqs. (5.2.6) implies that V has
full rank. Since its columns are orthogonal to the columns of U, the columns of U and V span
R ngc . As shown in Section 3.5.2, Eq. (3.5.23), V and U are related by the identity
VV T + U( U T U )-1 U T = I (5.2.7)
A tangent space parameterization of the constraint manifold of Eq. (5.2.4), in a
neighborhood of (q 0 , t 0 ) , is defined by
q = q0 + Vv - Uu (5.2.8)
where the minus sign indicates a projection that is orthogonal to the tangent plane of the
constraint manifold at (q 0 , t 0 ) .
The variables v and u that are introduced in Eq. (5.2.8) comprise a set of local
generalized coordinates that are in one to one correspondence with q in a neighborhood of q 0 ;
i.e., they are equivalent to q. To see this, multiply Eq. (5.2.8) by V T and U T to obtain
v = V T (q - q 0 )
(5.2.9)
u = - ( U T U ) U T (q - q 0 )
-1
Thus, the parameterization of Eq. (5.2.8) is a homeomorphism; i.e., it is one to one, onto,
continuous, and with a continuous inverse in a neighborhood of (q0 , t 0 ) . It is clear from Eq.
(5.2.9) that at q = q 0 ,
v0 = 0
(5.2.10)
u0 = 0
To assure that q of Eq. (5.2.8) satisfies the holonomic constraints of Eq. (5.2.1), it is
required that
Φ ( q 0 + Vv - Uu ) - f (t) = 0 (5.2.11)
The derivative of the left side of this equation with respect to u, with v and t held constant, is
( ˆ ˆ - Uu
Φ qˆ 0 + Vv ˆ )= -Φq (q)U , or
u
Φ u ( q 0 ) = -Φq (q 0 ) U = - U T U º -( B0 ) -1 (5.2.12)
which is nonsingular. Since Fq (q) is a continuously differentiable matrix function of q,
B( q ) º ( Φ q ( q ) U )
-1
(5.2.13)
is nonsingular and continuously differentiable with respect to its arguments in a neighborhood

X0 of q0 .
354
Since Φ u (q ) = -Φ q (q )U = - B -1 (q ) is nonsingular in a neighborhood of q 0 and u = 0 is
a solution of Eq. (5.2.11) with v = 0 and t 0 , the implicit function theorem of Section 2.2.5
guarantees that Eq. (5.2.11) has a unique continuously differentiable solution
u = h( v,t) (5.2.14)
in a neighborhood (V0 ,T0 ) of ( v 0 , t 0 ) . Thus, Eq. (5.2.8) becomes
q( v, t) = q0 + Vv - Uh( v, t) (5.2.15)
which satisfies Eq. (5.2.1) for all v and t in a neighborhood (V0 ,T0 ) of ( v 0 , t 0 ) .
To see geometrically what Eq. (5.2.15) means and why the extent of the neighborhood
V0 is important, the geometry of choosing u to stay on the regular configuration space of Eq.
(5.2.4) is shown in Fig. 5.2.1. The vector q0 + Vv may be viewed as movement in the tangent
plane of the manifold, which must be modified by vector -Uu to yield q( v, t) on the manifold.
Equation (5.2.15) consolidates the two vectors into the continuously differentiable vector
function q( v,t) . It is clear from Fig. 5.2.1, however, that if v is large, the vector -Uu that is
perpendicular to the plane tangent to the manifold at q0 may not intersect the manifold and the
parameterization fails; i.e., generalized coordinates v are no longer valid. Fortunately, a large
condition number of B(q , t) , defined in Section 2.2.8, is an effective warning that such a
situation is impending.
Figure 5.2.1 Projection onto Constraint Manifold

The quantities B(q ) and h ( q , t) are shown to exist and be continuously differentiable
functions of their arguments. While they cannot be evaluated as explicit functions of their
arguments, they can be evaluated numerically as accurately as desired using iterative algorithms.
The vector h(q, t) can be evaluated using the Newton-Raphson method of Section 2.2.7 to solve
Eq. (5.2.11) for u, with given v and t; i.e.,
F u ( q ) Du i = -Φ q (q 0 ) UDu i = -F (q 0 + Vv - Uu i ) + f (t 0 ) (5.2.16)
( )
Using Eq. (5.2.13), the solution of Eq. (5.2.16) is Dui = B(q ) F(q 0 + Vv - Uui ) - f (t 0 ) , yielding
the iterative algorithm
ui+1 = u i + B(q) ( F(q0 + Vv - Uui ) - f (t 0 ) ) , i = 1,2,L until F(q0 + Vv - Uu i +1 ) - f (t 0 ) £ utol
(5.2.17)
355
where utol is a specified error tolerance. Since the Newton-Raphson method does not require an
exact Jacobian, the matrix B( q ) is held constant for the process. Since only matrix
multiplication is required, this is an efficient computation.
The matrix B( q ) must be updated with changes in u and v , hence changes in q . To
enhance computational efficiency, B 0 = ( F 0qF 0q T )
-1
is evaluated at (q 0 ) and an iterative method
is used to update B( q ) = ( F q ( q )U )
-1
as the computation process proceeds. For q ¹ q 0 , B(q, t)
must satisfy Eq. (5.2.13). In residual form, suppressing arguments q for notational convenience,
this is
R = FqUB - I = 0 (5.2.18)
With an approximation B i of the solution, Newton-Raphson iteration can be carried out,
( F U ) DB = -R = -F UB + I i = 1, 2, L
q
i i
q
i
(5.2.19)
Since the matrix ( F U ) need not be inverted with great precision for use in the Newton-
q
Raphson process and B » ( F U ) , Eq. (5.2.19) yields DB = -B F UB + B . This is the

i -1 i i i i
q q
efficient iterative algorithm

Bi +1 = 2Bi - B i F q UB, i=1,2,L , until F q UB i +1 - I £ Btol (5.2.20)
where Btol is a specified error tolerance. Equation (5.2.20) may be executed whenever the test
Fq UBi - I £ Btol fails, an efficient computation that requires only matrix multiplication. In this
way, the foregoing kinematic equations and numerous more to follow with coefficient matrix
F u = -F q U = -B -1 can be solved using a multiplication by B.
5.2.2 Kinematic Relations

Differentiating Eq. (5.2.15) with respect to t, recalling that V and U are constant,
q& = Vv& - Uh& (5.2.21)
To evaluate h& , the derivative of Eq. (5.2.11) with respect to t, with u = h( q, t) and suppressing
arguments, is Φ q ( Vv& - Uh& ) - ft (t) = 0 . Using Eq. (5.2.13), this yields
h& = B ( Φ q Vv& - f t (t) ) (5.2.22)
and Eq. (5.2.21) is

q& = Vv& - UB(q )Fq (q )Vv& + UB(q )f t (t)
(5.2.23)
= D(q )v& + UB(q)ft (t)
where
D(q ) º ( I - UB(q )Φq (q ) ) V (5.2.24)
356
At t 0 , multiplying Eq. (5.2.23) on the left by V T and using the facts that VT U = 0 and
D(q0 ) = V - UΦq (q0 )V = V ,
v& 0 = V Tq& 0 (5.2.25)

An important property of D(q ) is obtained using Eq. (5.2.13), suppressing arguments,
Φ q D = ( Φ q - Φ q UBΦ q ) V = ( Φ q - Φ q ) V = 0 (5.2.26)
This shows that D is in the null space of the constraint Jacobian; i.e., its columns comprise a
continuously differentiable basis for the null space of Φ q . Multiplying both sides of Eq. (5.2.23)
on the left by the constraint Jacobian, Φq q& = Φq Dv& + Φq UBf t = ft , so q& of Eq. (5.2.23) satisfies
the velocity constraint equation of Eq. (5.2.2), for all v Î V0 , t Î T0 , and v& .
Differentiating Eq. (5.2.23) with respect to t,
&& = Dv
q ( )
&& + D(q)vˆ& q& + U B(q)fˆt
q
( ) q& + UBf
q
tt (5.2.27)
Writing Eq. (5.2.13) in the form Φq UBc = c , for a constant vector c, and differentiating with
( ˆ ˆˆ
respect to q, Φ q (q )UBc )
q
+ Φq U ( B(q)cˆ )q = 0 . Using Eq. (5.2.13),
( B(q)cˆ )q = -B ( Φq (q)UBc
ˆ ˆ ˆ)
q
(5.2.28)
Differentiating D(q)a of Eq. (5.2.24) with respect to q, where a is a constant vector and using
Eq. (5.2.28),
( D(q )aˆ )q = -U ( B(q )Φˆ q Va

ˆ ˆ ) - UB ( Φ (q )Va
q
q
ˆ ˆ)
q
( ˆ ˆ ˆ Va
= UB Φ q (q)UBΦ q
ˆˆ ( ) q
ˆˆ
- UB Φ q ( q )Va ) q
(5.2.29)
= - UB ( Φ (q) ( Va
q
ˆ ˆ - UBΦ ˆ ˆ ))
ˆ ˆ ˆ Va
q
q
= - UB ( Φ (q)Da
q
ˆ ˆ)
q
Substituting from Eqs. (5.2.28) and (5.2.29) into Eq. (5.2.27) and collecting terms,
q = Dv
&& (( )
ˆ &ˆ q& - f = Dv
&& - UB Φq (q)Dv tt
q
)
&& - UB Φq (q)q&ˆ q& - ftt (( )
q
) (5.2.30)
= Dv
&& - UBγ
where Eq. (5.2.3) has been used. Multiplying on the left by Φ q and using Eqs. (5.2.13) and
(5.2.26),
q = Φq Dv
Φq&& && - Φq UBγ = -γ (5.2.31)
&& of Eq. (5.2.27) satisfies Eq. (5.2.3), for all v Î V0 , t Î T0 , v& , and &&
Thus, q v.
357
The importance of the fact that generalized coordinates q given by Eq. (5.2.15) satisfy the
holonomic constraint of Eq. (5.2.1) and &q and&&q given by Eqs. (5.2.23) and (5.2.30) satisfy the
velocity and acceleration constraints of Eqs. (5.2.2) and (5.2.3) cannot be overemphasized. This
aspect of the tangent space formulation is critical in accurately satisfying holonomic
configuration, velocity, and acceleration constraints, avoiding so called constraint drift that is
associated with many multibody dynamics formulations and their numerical implementations
(Bauchau and Laulusa, 2008). The cost of achieving this important benefit is the computational
burden of carrying out iterative solutions for u and B in Eqs. (5.2.17) and (5.2.20). This
computational cost will be assessed in numerical solution of the equations of motion.
Showing the functional relationships of Eqs. (5.2.15), (5.2.23), and (5.2.30),
q(v, t) = q0 + Vv - Uh( v,t) (5.2.32)
q& (v, v& , t) = D(q( v )) v& + UB(q( v ), t)ft (t) (5.2.33)
&& v, t) = D(q( v, t))&&
q(v, v& , && v - UB(q( v, t), t)g( q( v, t), q& ( v, v& , t),t ) (5.2.34)
Eqs. (5.2.32) through (5.2.34) demonstrate that q, q& , and q
&& are differentiable functions of
v and provide mappings to determine q, &q, and&&q that satisfy Eqs. (5.2.1) through
v, v& , and &&
(5.2.3), once independent coordinates v, &v, and&&v are determined as a solution of some form of
the equations of dynamics.
5.2.3 Tangent Space Kinematics with Time Independent Constraints
The case of holonomic constraints of the form
Φ(q ) = 0 (5.2.35)
is common in applications. Since the foregoing analysis that accounts for time dependence in Eq.
(5.2.1) is intricate, it is important to summarize results for constraints of the form of Eq. (5.2.35).
Clearly, Φ t (q) = 0 , Φ tq (q) = 0 , and Φ tt (q ) = 0 . Thus, the term γ in Eq. (5.2.3) reduces to
γ(q,q) (
& = Fq (q, ˆt )q&ˆ q& ) q
(5.2.36)
The matrix B(q) of Eq. (5.2.13) is
B( q ) = ( Φ q ( q ) U )
-1
(5.2.37)
and the iterative algorithm for its evaluation remains as Eq. (5.2.20). The vector function of Eq.
(5.2.14) reduces to
u = h( v ) (5.2.38)
and the iterative algorithm for its evaluation remains as Eq. (5.2.17), with f (t 0 ) = 0 . The matrix
D(q ) of Eq. (5.2.24) that appears often is
D(q ) º ( I - UB(q)Φq (q ) ) V (5.2.39)
and the results of Eqs (5.2.32) through (5.2.34) reduce to
358
q(v) = q0 + Vv - Uh( v ) (5.2.40)
q& (v, v& ) = D(q( v ))v& (5.2.41)
&& v ) = D(q( v ))&&
q(v, v& , && v - UB(q( v ))g( q( v ), q& ( v, v& )) (5.2.42)
Thus, results of the derivation of Section 5.2.1 for the common special case of Eq.
(5.2.35) reduce gracefully to Eqs. (5.2.36) through (5.2.42).
5.2.4 ODE of Motion
Based on the tangent space kinematics formulation, the variational equations of motion
for holonomic systems of Section 4.6 are to be reduced to a system of ODE. The variational
equation of motion for a multibody system of Eq. (4.6.16) is
δq T ( M (q)q
&& - S(q, q& ) - QA (q, q& , t) ) = 0 (5.2.43)
which must hold for all δq that satisfy F q δq = 0 , for a system with holonomic constraints of Eq.
(5.2.1) that include Euler parameter normalization conditions for spatial bodies. As in Eq.
(5.2.23), with time held fixed,
dq = D(q )dv (5.2.44)
Using Eqs. (5.2.42) and (5.2.44), Eq. (5.2.43) reduces to
δvT DT (q ) ( M(q )D(q)&&
v - M(q)UB(q)g (q, q& ,t) - S(q, q& ) - QA (q, q& , t) ) = 0 (5.2.45)
From Eqs. (5.2.26) and (5.2.44),

F q (q )δq = F q (q )D(q )dv = 0 (5.2.46)
for arbitrary dv . Thus, Eq. (5.2.45) holds for arbitrary dv and, suppressing arguments,
&& = DT ( MUBg + S + QA )
DT MDv (5.2.47)
where Eqs. (5.2.32) and (5.2.33) show that q and q& are functions of v and v& ; i.e.,
M(q(v, t)) = M(v, t)
& (v,&v, t), t) = g (v,&v, t)
g (q(v, t),q
S(q(v, t),q& (v,&v, t), t) = S(v,&v, t)
(5.2.48)
Q A (q(v, t)&q(v,&v, t), t) = Q A (v,&v, t)
B(q(v, t)) = B(v, t)
D(q(v, t)) = D(v, t)
Equation (5.2.47) is thus a second order ODE in v.
At an initial configuration (q 0 , t 0 ) that satisfies Eq. (5.2.1), F q (q0 )V = 0 , so
D0 = D(q0 ) = V - UB(q0 )F q (q0 )V = V . Thus, D0T M (q 0 ) D 0 = V T M (q 0 ) V . Since the columns
of V form a basis for the null space of F q (q0 ) , on which M (q 0 ) is positive definite, the
359
symmetric reduced mass matrix D T (v , t)M (v , t)D (v , t) of Eq. (5.2.47), which is a continuous
function of v and t, is positive definite, hence nonsingular, in a neighborhood of ( v 0 , t 0 ) .
Multiplying both sides of Eq. (5.2.47) by the inverse of D T MD , suppressing arguments of
functions involved for simplicity, yields the traditional form of a second order ODE,
v = ( DT MD ) DT ( MUBg + S + Q A ) º g ( v, v& , t)
-1
&& (5.2.49)
Given initial values of q(t 0 ) and q& (t 0 ) that satisfy Eqs. (5.2.1) and (5.2.2) at t 0 , the first
of Eqs. (5.2.10) and Eq. (5.2.25) yield initial conditions on v and v& ,
v (t 0 ) = 0
(5.2.50)
v& (t 0 ) = V T q& (t 0 )
Assuming all functions appearing in Eq. (5.2.48) are continuously differentiable with
respect to their arguments, it is shown in Section 4.7.3 that the initial-value problem of Eqs.
(5.2.49) and (5.2.50) has a unique solution for v (t) in a neighborhood of ( v 0 , t 0 ) . It is shown in
Section 3.6 that this solution can be continued on the constraint manifold, as shown
schematically in Fig. 5.2.2, until a singular configuration or the desired final time tf is
encountered. Further, if terms in Eq. (5.2.48) are k-times differentiable with respect to design
parameters b, so is the solution. With these existence, uniqueness, and differentiability
properties, the initial-value problem of Eqs. (5.2.49) and (5.2.50) is well posed.
Figure 5.2.2 Continuation of Solution on Constraint Manifold

It is important to note that Eq. (5.2.47) is a second order ODE. It is not a DAE such as
Eq. (4.10.8) that is encountered if Lagrange multipliers are introduced, nor is it a differential
equation on a manifold (Hairer, Lubich, and Wanner, 2006). The tangent space formulation thus
avoids analytic and computational difficulties associated with both of these classes of equations.
It is interesting to note that the ODE formulation has been derived using d’Alembert’s principle,
without introducing Lagrange multipliers. It is shown in Section 5.11 that the tangent space ODE
formulation is equivalent to variational and DAE formulations.
Unlike the analytical reduction for the top of Section 5.1.2, numerical reduction of the
variational equations of motion to ODE depends on tolerances utol in approximating u and Btol
in approximating B in Eqs. (5.2.17) and (5.2.20). Thus, after numerical solution of reduced
360
equations of motion for v and its derivatives, Eqs. (5.2.36) through (5.2.38) yield q, q& , and q
&& that
satisfy the constraints of Eqs. (5.2.1) through (5.2.3). More specifically, the accuracy with which
the configuration generalized coordinates q are evaluated depends on the accuracy with which u
is computed in Eq. (5.2.19); i.e., utol. As long as convergence in this iteration is achieved, the
accuracy with which B is computed in Eq. (5.2.22) is immaterial. In contrast, the accuracy of q&
and q&& that are computed in Eqs. (5.2.37) and (5.2.38) is directly impacted by the accuracy with
which B is computed in Eq. (5.2.23); i.e., Btol. Thus, both utol and Btol control the accuracy
with which the velocity and acceleration generalized coordinates are evaluated.
Finally, it is noted that with the unique solution v (t) and its derivatives known, Eqs.
(5.2.32) through (5.2.34) determine the unique solution q (t) and its derivatives of the multibody
dynamics problem. As emphasized earlier, this solution satisfies all three forms of the holonomic
constraint equations of Eqs. (5.2.1) through (5.2.3). Thus, problems associated with drift from
constraints that arise in many numerical solutions of the DAE of multibody dynamics (Bauchau
and Laulusa, 2008) are no longer of concern.
5.2.5 Calculation of Constraint Reaction Forces
A note is in order regarding the coupling of kinematics and dynamics. The Lagrange
multiplier form of equations of motion of Eq. (4.10.8), with Euler parameter normalization
conditions included in the holonomic constraint equations F (q ) - f (t) = 0 , is
&& + ΦqT (q)λ - QA (q, q& , t) - S(q, q& ) = 0

M(q)q (5.2.51)
It is shown in Section 4.10.4 that, once a solution q (t) is known, the Lagrange multiplier l
determines constraint reaction forces and may be obtained by factoring the matrix FqFq at each
T
solution time. A much more efficient calculation may be made in the tangent space formulation.
Multiplying Eq. (5.2.51) by U = Fq yields
T 0
UTFqT (q)l = UT ( M(q)&&

q + S(q, q& ) + QA (q, q& , t) ) (5.2.52)
( )
Writing Eq. (5.2.13) in the form Φq (q)UB(q ) and taking the transpose, BT (q ) U TFqT (q) = I .
Thus, BT (q) is the inverse of the coefficient matrix of l in Eq. (5.2.52) and, by a simple
multiplication,
l = BT (q, t)UT ( M(q)q
&& + S(q, q& ) + QA (q, q& , t) ) (5.2.53)
This calculation can be performed at each time step in numerical integration of the equations of
motion to inexpensively determine constraint reaction forces.
361
Broadly applicable tangent space generalized coordinates are shown to satisfy all three forms
of kinematic constraint, for arbitrary values of independent coordinates. The inverse B of the
holonomic constraint subJacobian with respect to dependent coordinates is efficiently updated
for evaluation of dependent coordinates to prescribed accuracy. It also provides for efficient
calculation of numerous quantities in this and subsequent sections of the chapter. Finally, it
enables efficient evaluation of constraint reaction forces in the Lagrange multiplier form of
equations of motion derived in Section 4.10.
Efficient means of updating the tangent space parameterization yield a family of local
parameterizations, or charts, on the constraint manifold that enable systematic continuation of
solution trajectories, as outlined in Section 3.6. Since a global parameterization is generally not
possible with a single set of independent generalized coordinates, the tangent space approach is
among the only options available to obtain a globally valid ODE formulation for mechanical
system dynamics.
Key Formulas
F (q, t )F (q) - f (t) = 0 F q (q)q& = ft (q, t ) F q (q)q
&& = - γ (5.2.1) (5.2.2) (5.2.3)
U º FqT (q0 ) Fq (q0 )V = 0 VTV = I UT V = 0 (5.2.5) (5.2.6)
q = q0 + Vv - Uu (5.2.8)
Du i = B ( F(q0 + Vv - Uui ) - f (t 0 ) )
u = h ( v, t) i = 1,2,L (5.2.14) (5.2.17)
ui+1 = ui + Du i
DBi = -Bi Fq UBi + Bi

B º (FqU )
-1
i=1,2,L (5.2.13) (5.2.20)
Bi +1 = Bi + DBi
q(v, t) = q0 + Vv - Uh( v, t) D(q ) º ( I - UB(q )Φq (q ) ) V (5.2.32) (5.2.24)
q& (v, v& , t) = D(q( v, t))v& + UB(q( v, t))ft (t) (5.2.33)

&& v, t) = D(q( v,t))&&
q(v, v& , && v - UB(q( v,t)) g( q( v, t), q& ( v, v& , t), t ) (5.2.34)
&& = DT ( MUBg + S + QA )
DT MDv (5.2.47)
v(t 0 ) = 0 v& (t 0 ) = V Tq& (t 0 ) (5.2.50)
362
5.3 Numerical Solution of Tangent Space ODE
Explicit Runge-Kutta integration algorithms are presented in Section 5.3.1, using results
of Section 4.8, for solution of the ODE initial-value problem of Section 5.2. Derivatives required
to implement implicit integration methods of Section 4.8 are derived in Sections 5.3.2 and 5.3.3
and implicit numerical integration algorithms are presented in Sections 5.3.4 through 5.3.6 for
solution of the equations, including computation of third derivatives for use in error control.
5.3.1 Explicit Numerical Integration Algorithm
The process of integrating the ODE initial-value problem of Eqs. (5.2.49) and (5.2.50),
repeated here as
&& = D T ( MUBg + S + Q A )
DT MDv (5.3.1)
v(t 0 ) = 0
(5.3.2)
v& (t 0 ) = V T q& 0
requires evaluation of all terms in Eq. (5.3.1), the most demanding of which is g ( q, q& ,t) defined
( )
in Eq. (5.2.3). Evaluation of P2(q, q& ) = F (q )q&ˆ for planar and spatial constraints is carried out
q
q
using differentiation results presented in Appendix 5.B. Once a solution for v(t) and its time
derivatives are obtained, Eqs. (5.2.33) through (5.2.35), repeated here as
q(v, t) = q0 + Vv - Uh( v,t) (5.3.3)
q& (v, v& , t) = D(q( v, t))v& + UB(q( v, t))f t (t) (5.3.4)
&& v, t) = D(q( v, t))&&
q(v, v& , && v - UB(q( v, t))g( q( v, t), q& ( v, v& ,t), t ) (5.3.5)
are used to recover the solution q (t) and its time derivatives.
Explicit numerical integration of the initial-value problem of Eqs. (5.3.1) and (5.3.2),
using Runge-Kutta methods, is as follows:
(1) Define initial conditions q0 and q& 0 at t 0 that satisfy kinematic configuration and
velocity constraints of Eqs. (5.2.1) and (5.2.2). Evaluate the constraint Jacobian F q (q0 ) ,
matrices U and V of Eqs. (5.2.5) and (5.2.6), and g of Eq. (5.2.3). Obtain initial conditions
on v and v& from Eqs. (5.3.2) for integration of Eq. (5.3.1).
(2) Solve Eq. (5.3.1) for &v& and apply an explicit numerical integrator of Section 4.8.2.1 to
proceed on a time grid with step size h. This yields v, v& , and &&
v on the time grid. Use Eqs.
& &&
(5.3.3) through (5.3.5) to evaluate q, q , and q on the time grid.
(3) Monitor the condition number of the reduced mass matrix D T MD of Eq. (5.3.1), the
norm of v, and the number of iterations required to evaluate u and B in Eqs. (5.2.17) and
(5.2.20). If tolerances are satisfied, continue the process. If tolerances are exceeded, define a
new time t 0 and associated q 0 and q& 0 . Repeat calculations in Step (1) to define a new
parameterization and initial conditions v 0 and v& 0 of Eq. (5.3.2) to restart the integration
363
process. This process follows the trajectory shown in Fig. 5.2.2, moving smoothly across
charts on the constraint manifold.
(4) Continue the process until the final time tf is reached, or a singular configuration
associated with a faulty design or model occurs.
5.3.2 Derivatives for Implicit Integration
To use an implicit numerical integration method to solve Eqs. (5.3.1) and (5.3.2),
derivatives of all terms that appear with respect to v and v& are required. For the reader who is
satisfied with the use of explicit numerical integration methods, the somewhat intricate
derivation in the remainder of this section can be bypassed.
Since arguments of functions that appear in Eq. (5.3.1) are q (v, t) and q& (v, v& , t) ,
derivatives of q and q& with respect to v and v& are required. Differentiating Eq. (5.3.3) with
respect to v, with t constant,
q v = V - Uhv (5.3.6)
To evaluate h v , the derivative of Eq. (5.2.11) with respect to v, with u = h and t constant, is
Φ q ( V - Uh v ) = 0 , so h v = BΦq V . Substituting this into Eq. (5.3.6),
q v = V - UBΦq V = D (5.3.7)
Differentiating Eq. (5.3.4) with respect to v& ,

q& v& = D (5.3.8)
and with respect to v, using Eq. (5.3.6),
(
q& v = ( Dvˆ& ) q + U ( Bfˆt )q D ) (5.3.9)
Recall the definition of Eq. (3.1.19); i.e.,

P2( q, χ ) º ( F q (q ) χˆ ) (5.3.10)
q
where χ Î R ngc and Eqs. (5.2.24), (5.2.28), and (5.2.29) are
D(q ) º ( I - UB(q )Φq (q ) ) V (5.3.11)
( B(q )cˆ )q = -B ( Φq (q )UBc

ˆ ˆ ˆ)
q
(5.3.12)
( D(q )aˆ )q = -UB ( Φq (q)Da

ˆ ˆ)
q
(5.3.13)
Using these relations and Eq. (5.3.4), Eq. (5.3.9) may be written as
q& v = ( -UBP2(q, Dv& ) - UBP2(q, UBf t ) D = -UB ( P2(q, Dv& + UBft ) ) D
(5.3.14)
= -UBP2(q, q& )D
where linearity of P2(q, χ ) in its second argument and Eq. (5.3.4) have been used.
364
From Eq. (5.2.3),
(
γ q = ( F q ( q )qˆ& )q qˆ& ) = ( P2(q, qˆ& )qˆ& )
q q
(5.3.15)
γ q& = ( ( F ( q )qˆ& ) q& )
q q
q&
Using matrix product expansion,
( F q&ˆ )
q
1
q
æ
j è i
ö
øq j
æ
j è i
ö
q& 2 = å ç å F qi q&ˆ 1i ÷ q&ˆ 2j = å ç å F qi q j q& 1i ÷ q& 2j
ø
æ ö
æ
j è i
ö
= å ç å F q jqi q& 2j ÷ q& 1i = å ç å F q j qˆ& 2j ÷ q& 1i = F qqˆ& 2
ø i è j øq j
( ) q
q& 1
( )
so the quantity (Fq q&ˆ ) q q& is symmetric in its q& arguments. Thus,
(( F qˆ& ) q& ) = 2 ( F qˆ& ) = 2P2(q, q& )

q
q q&
q
q
(5.3.16)
Define the triple derivative with respect to q in the first of Eqs. (5.3.15) as
P3(q , q& ) º ((F (q)qˆ& ) q&ˆ ) = (( P2(q,q&ˆ ) q&ˆ )

q
q q q
(5.3.17)
Substituting Eqs. (5.3.16) and (5.3.17) into Eqs. (5.3.5) and (5.3.15), with the definition of Eq.
(5.3.10),
g = P2(q, q& )q& - f tt
γ q = P3(q, q& ) (5.3.18)
γ q& = 2P2(q, q& )
Taking the transpose of Eq. (5.2.13), written in the form BΦq U = I , and multiplying on
the right by a vector e yields U T F qT BTe = e . Differentiating both sides with respect to q, with e
constant,
( q )
ˆ ˆ + U TF T ( BT (q )eˆ ) = 0
U T F qT (q )Be
q q
(5.3.19)
Taking the transpose of Eq. (5.2.13), B T = ( U T F qT ) . Using this relation in Eq. (5.3.19) and
-1
defining
P4(q, η) º ( F Tq (q ) ηˆ ) (5.3.20)
q
where η Î R nhc , Eq. (5.3.19) yields
( B T (q )eˆ )q = - BT U T F qT (q )B(
ˆ Teˆ ) q
= - B T U T P4(q, BTe ) (5.3.21)
365
Taking the transpose of Eq. (5.3.11) and multiplying on the right by a vector d yields
D d = V Td - V T F qT BT U Td . Differentiating both sides and using Eqs. (5.3.19) and (5.3.20),
T
( D (q)dˆ )
T
q ((
ˆ T U Tdˆ
= -V T F qT (q)B ) q
(
+ F qT B T (q )U Tdˆ )) q
= -V T
( P4(q, B U d) - F B U P4(q, B U d) )
T T T
q
T T T T
(5.3.22)
= -V T
( I - F B U ) P4(q, B U d)
T
q
T T T T
= -DT P4(q, B T U Td)

The differential equation of Eq. (5.3.1) may be written in residual form as
R(&& && - DT ( MUBγ + S + QA ) = 0
v, v& , v) º DT MDv (5.3.23)
Numerical integration, using an implicit ODE integration formula, requires the derivatives
R &&v , R v& , and R v . Since terms on the right of Eq. (5.3.23), with the exception of &v& , depend on
q( v,t) and q& ( v, v& ,t) , the chain rule of differentiation may be used with the forgoing identities to
calculate the needed derivatives.
The derivative of the first term on the right of Eq. (5.3.23) with respect to q is
( D (q)M(q)D(q)&&vˆ ) = ( D (q)MDv
T ˆ ˆ &&ˆ )
q
T
q
+ DT M(q )Dv(
ˆ &&ˆ + DT M D(q )&&
vˆ ) q
( ) q
(5.3.24)
Since M(q) is symmetric and using Eq. (5.3.13),
( D (q)MDv
T ˆ ˆ &&ˆ ) = ( D
q
ˆ MD vˆ )
ˆ (q )&& T
q
vˆ )q = -DT MUBP2(q, Dv
= D T M( D(q)&& &&)
Defining
M 2( q, μ ) º ( M (q )μˆ )q (5.3.25)
where μ Î R ngc , Eq. (5.3.24) reduces to
( D (q)M(q)D(q)&&vˆ )
T
q
= -2DT MUBP2(q, Dv
&&) + D T M 2(q, Dv
&&) (5.3.26)
Using the foregoing derivative formulas and suppressing arguments, derivatives of the
second term on the right of Eq. (5.3.23) with respect to q and q& are
( D (q) ( M(q)UB(q)γ(q, qˆ& , t) + S(q,qˆ& ) + Q (q, q&ˆ , ˆt)))

T A
q
( ) (
= D T (q)( MUBγ + S + Q A ) + D T M(q)UB(q )γ (q, qˆ& , t) + S(q, q&ˆ ) + Q A (q, qˆ& , ˆt)
·
q
) q (5.3.27)
= - DT P4(q, B T U T ( MUBγ + S + Q A ))
(
+ D T M 2(q, UBγ ) - MUB ( P2(q, UBγ ) - γ q ) + Sq + QqA )
and
366
DT ( M(qˆ )UB(q) γ(qˆ , q& , t) + S(qˆ , q& ) + Q A (qˆ , q& , ˆt) ) & = DT ( MUBγ q& + Sq& + QqA& ) (5.3.28)
q
With the results of Eqs. (5.3.26), (5.3.27), (5.3.28), (5.3.6), (5.3.8), and (5.3.13), and
suppressing arguments,
R &&v = D T MD
R v& = - DT ( MUBγ q& + Sq& + QqA& ) D (5.3.29)
ì- MUBP2 ( q,2Dv && - UBg ) + P4 ( q, BT U T ( MUBγ + S + Q A ) ) ü
ïï ïï
R v = D T í+ M 2 ( q, Dv&& - UBγ ) - MUBg q - Sq - QqA ýD
ï ï
ïî+ ( MUBγ q& + Sq& + Qq& ) UBP2 ( q, q& )
A
ïþ
5.3.3 Organization of Derivative Calculation
The foregoing derivative calculations are expanded, as in Section 3.1, for constraints
F (qi , q j ) - f (t) = 0 between pairs of bodies. From Eq. (5.3.17),
(
P3(q i j , q& i j ) = P 2(q i j , q&ˆ i j )q&ˆ i j ) qij
é
(
= ê P 2(q i j , q&ˆ i j )q&ˆ i j
ë
) ( P2(q , q&ˆ
qi ij ij
)q&ˆ i j ) qj
ù
úû (5.3.30)
and, from Eqs. (3.1.16) and (3.1.21),
( P2(q , q&ˆ )q&ˆ )

ij ij ij
qi
(( ) q&ˆ )
= F qi q&ˆ i
qi
i
qi
æ
è
(
+ 2 ç F qi q&ˆ i ) qj
ö æ
øqi è
(
q&ˆ j ÷ + ç F q j q&ˆ j ) qj
ö
q&ˆ j ÷
øqi
= ( ( F qˆ& ) qˆ& )
(5.3.31)
( P2(q i j , qˆ& i j )qˆ& i j ) qj
qi i
qi
i
qj è
(
+ 2 æç F qi qˆ& i ) qj
æ
øq j è
(
qˆ& j ö÷ + ç F q j qˆ& j ) qj
ö
qˆ& j ÷
øq j
To evaluate the operator of Eq. (5.3.20),
(
é F qT hˆ ù é F Tq h
)
ˆ ( ) ( F hˆ ) T
ù
( )
qi
ê qi ú qj
=ê ú =
i
P 4(qi j , h) = F h
ˆ
i
T
(5.3.32)
(
ê F T hˆ ú ê T
) ( ) ( ú
)
qij
qi j F hˆ F q j hˆ ú
T
ë q j û q i j êë q j qi qj û
where four derivative expressions on the right are identified. Expanding matrix products and
derivatives, with row index m and column index n,
( F hˆ )
T
qi
qj
α
(
= å F qαi T hˆ α ) qj
é
ë α
(
= ê å F qα im T hˆ α ) q jn
ù é
ú
ù é
= ê å F qαimq jm T hˆ α ú = ê å F qα jn T hˆ α
û ëα û ëα
( ) ù
qim ú
û
(5.3.33)
Interchanging row and column indices m and n yields the transpose relation
é
( ) ù
( ) = ( F hˆ )
T
æ Tˆ
è
(
ç F qi h ) ö
÷ = ê å F q jm hˆ α
qj ø
ë α
α T
q in ú = å F qα j T hˆ α
û α qi
T
qj
qi
(5.3.34)
Thus, only three of the four terms on the right of Eq. (5.3.32) are independent; i.e.,
367
(
é F Tq hˆ ù )
é F Tq h ˆ ( ) ( F hˆ )
T
qi
ù
( )
ê i qi qj ú
P4(qi j , h) = F Tq h =ê ú =ê
i
ˆ ú (5.3.35)
(
ê FT h ˆ ú ) ( )
T
qi j
ê æ FT h ö
( ) ˆ ú
ij
ë qj ç ˆ
û q i j êè qi q j ø ÷ F Tq j h
ë qj ú
û
The first equality in Eq. (5.3.33) provides a helpful identity in evaluating terms in
P 4(qi j , h) of Eq. (5.3.35). For constraints that are comprised of a column of building block
constraint expressions F a (q i , q j ) - f a (t) = 0 and associated factors ha , entries in the matrix of
Eq. (5.3.35) are comprised of the sum of terms that are calculated for individual building block
constraints; i.e.,
( F hˆ ) = å ( F
T
qi
qj
α T
qi h
ˆα )
qj
(5.3.36)
α
Evaluation of kinetic derivatives is less intricate. Since

M (q) = diag ( M1 (q1 ),L , M nb (q nb ) ) (5.3.37)
is block diagonal in terms associated with individual bodies,
(
M 2(q, μ ) º diag ( M1μˆ 1 ) ,L , ( M nb μˆ nb )
q1 q nb ) (5.3.38)
Similarly, but less structured in generalized coordinate dependence, the virtual work definition of
generalized applied force,
dW A = å δqiTQiA (q, q& , t) (5.3.39)
i
identifies components of generalized force with bodies, but each component may depend on the
full range of generalized coordinates and velocities. Thus,
é Q1Aq ù é Q1Aq& ù
ê ú ê ú
QqA = ê M ú QqA& = ê M ú (5.3.40)
êëQ Anbq úû êëQ Anbq& úû
where nb is the number of bodies in the system. Velocity coupling terms are associated with
individual bodies and their generalized coordinates and velocities; i.e., S i = S i (qi , q& i ) . Thus,
(
Sq = diag S1q1 ,L , Sqnbnb )
(5.3.41)
Sq& = diag ( S 1
q& 1 ,L , Sqnb
& nb )
Derivatives of Eqs. (5.3.30), (5.3.31), and (5.3.32) for constraints that are defined in
Sections 3.2 and 3.3 are summarized in Appendix 5.B. Kinetic terms in Eqs. (5.3.38) and
(5.3.41) are summarized in the same appendix. Since the dependence of generalized force terms
on generalized coordinates and velocities is difficult to anticipate, their derivatives are left for
specific applications.
368
5.3.4 Trapezoidal Implicit Numerical Integration
Trapezoidal second order integration formulas of Eq. (4.8.40) for v and v& are
v n = v n-1 + hv& n-1 + (h 2 / 4)(&&
v n -1 + &&
vn )
(5.3.42)
v& n = v& n-1 + (h/2)( &&
v n -1 + &&
vn )
Equations (5.2.15) and (5.2.23) are used to evaluate q n and q& n , where h n = u n is evaluated
using Eq. (5.2.17) and B( q n ) is evaluated using Eq. (5.2.20). The residual R of Eq. (5.3.23) is
thus a function of only &v& n and the chain rule of differentiation yields the Jacobian of the
trapezoidal residual with respect to &v& n ,
dR ¶v& ¶v
J trap = = R &&v + R v& n + R v n = R &&v + (h/2)R v& + (h 2 / 4) R v (5.3.43)
d&&
vn ¶&&
vn ¶&&
vn
With initial conditions q (t 0 ) = q 0 and q& (t 0 ) = q& 0 that satisfy F (q 0 ) - f (t 0 ) = 0 and

F q (q0 )q& 0 = f t (t 0 ) , initial values v 0 = 0 and v& 0 = V T q& 0 are obtained from Eqs. (5.3.2) . To obtain
an estimate of &v& (t 0 ) that is needed to begin iterative solution of the residual equation, Eq.
(5.3.23) may be solved with given initial conditions for &v& (t 0 ) . In subsequent time steps, the
estimate &&v 0n =&&v n -1 may be used.
To carry out Newton-Raphson iteration for &v& n ,
v n-1 )D&&
J trap (&& v in = - R (&&
v ni )
i = 0,1,L until R (&&
vin ) £ intol (5.3.44)
&&v n = &&
i+1
v n + D&&v n
i i
where intol is the numerical solution error tolerance.

Since a precise Jacobian is not required in Newton iteration, criteria based on the number
of iterations to achieve convergence in Eq. (5.3.44) may be used to determine when the relatively
costly process required to compute a new value of the integration Jacobian J trap is carried out. If
the integration Jacobian is to be updated, partial derivatives of the residual in Eqs. (5.3.29) must
be evaluated and the Jacobian of Eq. (5.3.43) computed.
Much as B = ( Φq (q )U ) is evaluated at q0 as B0 = ( U T U ) and iteratively updated using

-1 -1
the Newton-Raphson method applied to ( Φ q (q )U ) B(q ) = I in Eq. (5.2.22), the inverse of the
Jacobian may be evaluated. At t 0 , J 0inv º J (q(t 0 ))-1 and J inv is updated at time step t n as the
solution of JJ inv = I , where J = J (q(t n )) , using Newton-Raphson iteration with the estimate
j+1
J 0inv = J inv (q(t n-1 )) ; i.e., with R = JJ inv - I and DJ inv
j
= J inv - J inv
j
,
JDJ inv
j
= - ( JJ inv
j
- I ) , j = 0,1,... (5.3.45)
Since J -1 » J inv
j
, DJ inv
j
= - J inv
j
( JJ invj - I ) = J invj - J invj JJ invj , and
j+1
J inv = 2J inv
j
- J inv
j
JJ inv
j
, j = 0,1,... until JJ inv
j
- I £ intol (5.3.46)
369
With J inv » ( J trap (&&
vn ))
-1
so determined, Eq. (5.3.43) yields D&&
v in = -J inv R (&&
v in ) , or
n = v n - J inv R ( v n ) , i = 1,..., until R ( v n ) £ intol

&&
v i+1 && i && i && i (5.3.47)
To implement the integration error control approach presented in Section 4.8.5, in the
present case,
v n = v n -1 + hv& n -1 + (h 2 / 2)&&
v n -1 + (h 3 / 6)&&&
v n -1
the third derivative &&&
v n must be computed. For systems in which the variable t does not appear
explicitly, Eq. (5.3.1) may be differentiated to obtain
DT MDv (
&&&n -1 = - D T MDv ) (
&& q& n -1 + D T (MUBg + S + Q A ) q& n -1 + DT MUBg + S + Q A
q
) q
( ) q&
&& n-1
q
where all terms are evaluated at t n -1 . Substituting from Eqs. (5.3.26) and (5.3.28) and collecting
terms, this yields
D T MDv (
&&&n -1 = D T MUBg q& + Sq& + QqA& q
&& n -1 + Aq& n -1 º Rhs) (5.3.48)
where
é MUBP2 ( q, 2Dv&& n -1 - UBg ) - M 2 ( q, Dv
&& n -1 - UBg ) ù
A º DT ê ú
T T
(
êë - P4 q, B U (MUBg + S + Q ) + MUBg q + Sq + Q q úû
A A
)
While there are numerous terms in Eq. (5.3.48), all must be evaluated in each iteration of Eq.
(5.3.44), so the only cost in solving for &&&
v n -1 is the solution of Eq. (5.3.48). This is not a
significant additional cost, as indicated in Section 4.8.5.
In addition to providing information for error control, the third derivative &&&
v n -1 enables
improved accuracy of the estimate for the solution variable &v& n , namely
v n » &&
&& v n -1 + h&&&
v n -1 (5.3.49)
5.3.5 Runge-Kutta Implicit Numerical Integration
Stage i of the Runge-Kutta method of Eq. (4.8.27) is applied to Eq. (5.3.1) to obtain
i -1
v ¢i º v n + hci v& n + h 2 å A ijk j
j=1
i -1
v& ¢i º v& n + h å a ijk j
j=1 (5.3.50)
k i = g ( t n + c i h,v ¢i + h 2 g 2 k i ,v& ¢i + hgk i )
= (( D MD)
T -1
)
D T ( MUBg + S + Q A ) ( v ¢i + h 2 g 2 k i , v& ¢i + hgk i , t n + c i h)
where q i and q& i are evaluated as outlined for the trapezoidal method. Multiplying both sides on
the left by D T MD yields the residual form of the third of Eqs. (5.3.50),
370
Rº (( D MD ) (v¢ + h g k , t + c h) ) k
T
i
2 2
i n i i
(5.3.51)
- ( D ( MUBg + S + Q ) ) ( v¢ + h g k , v& ¢ + hgk , t
T A
i
2 2
i i i n + ci h) = 0
The Jacobian of Eq. (5.3.51) is thus

dR
J RK = = R &&v + hgR v& + h 2 g 2 R v (5.3.52)
dk i
With initial conditions q (t 0 ) = q 0 and q& (t 0 ) = q& 0 that satisfy F (q 0 , t 0 ) = 0 and

F q (q0 ,t 0 )q& 0 = -Φt (q0 ,t 0 ) , initial values v 0 = 0 and v& 0 = V T q& 0 are obtained from Eqs. (5.3.2) .
To obtain an estimate for k10 that is needed to begin iterative solution of the residual equation,
Eq. (5.3.51) may be solved with initial conditions and h = 0. Newton-Raphson iteration for k i is
then carried out as in Eq. (5.3.44); i.e., at time t i , with an estimate k i0 = k i -1 ,
J RK ( v n -1 ) Dk ij = - R (k ij )
j = 0,1,L until R (k ij+1 ) £ intol (5.3.53)
k ij+1 = k ij + Dk ij
where intol is a solution error tolerance. To evaluate R (k ij ) of Eq. (5.3.53), Eqs. (4.8.28) are
used to obtain v in and v& in , Eq. (5.2.17) is solved for uin , Eq. (5.3.3) is used to obtain qin , Eq.
(5.2.25) is used to update B, and Eqs. (5.3.4) and (5.3.5) are used to determine q& in and &q&in .
Since a precise Jacobian is not required in Newton iteration, criteria based on the number
of iterations to achieve convergence in Eq. (5.3.53) may be used to determine when the relatively
costly process required to compute a new value of the integration Jacobian J is justified. If the
integration Jacbian J is to be updated, partial derivatives of the residual in Eqs. (5.3.29) must be
evaluated and the Jacobian of Eq. (5.3.52) computed. As in the trapezoidal algorithm of Section
5.3.4, the inverse of J RK can be iterative evaluated. This leads to the modified iterative algorithm
of Eq. (5.3.47). The integration error control and step size algorithm of Eqs. (4.8.35) to (4.8.37)
in Section 4.8 may be used in implementing the Runge-Kutta method.
5.3.6 Implicit Numerical Integration Algorithm
Implicit integration algorithm for Eqs. (5.3.1) and (5.3.2), using trapezoidal and
Runge-Kutta methods is as follows:
velocity constraints. Evaluate the constraint Jacobian F q (q0 ) and matrices U and V in Eqs.
(5.2.5) and (5.2.6). Evaluate initial conditions v 0 = 0 and v& 0 = V T q& 0 .
(2) Apply an implicit numerical integrator to proceed stepwise on a time grid with step size
h, using a factored form of the integration Jacobian of Eq. (5.3.43) or (5.3.52) to iteratively
determine &&v n or k n . Use Eq. (5.3.42) or (4.8.28) to determine v n and v& n . Use Eqs. (5.3.3)
through (5.3.5) to evaluate q n , q& n , and &&
q n on the time grid.
371
(3) Monitor the condition number of B, as a measure of ill conditioning of the constraint
equations. If the condition number exceeds a limit, say 103 , define a new time t 0 and
associated q 0 . Repeat calculations in Step (1) to define a new parameterization and initial
conditions v 0 and v& 0 , and restart the simulation. As secondary indicators of the need to
reparameterize, monitor the number of Newton-Raphson iterations required in Step 2, the
norm of v, and the number of iterations required to evaluate u and B.
(4) Continue the process until the final time tf is reached, or a singularity is encountered due
to a faulty design or model.
ODE of motion in independent tangent space coordinates are integrated using explicit Runge-
Kutta numerical integration methods presented in Section 4.8, with minimal coding effort.
In order to support implicit numerical integration, derivatives of terms in the ODE of motion
are evaluated. While the formulas derived for this purpose are intricate, they may be
systematically programmed to implement the desired implicit numerical integration method.
Key Formulas
&& = DT ( MUBg + S + QA )
DT MDv v (t 0 ) = 0 v& (t 0 ) = V T q& 0 (5.3.1) (5.3.2)
P 2(q, χ ) º ( F q χˆ )
q ((
P3( q, q& ) º F q (q )qˆ& qˆ&) q
) q
(5.3.10) (5.3.17)
P 4(q, η) º ( F Tq ηˆ ) M 2(q, μ) º ( Mμˆ )q (5.3.20) (5.3.25)

q
g = P2( q, q& )q& - f tt

γ q = P3(q , q& ) (5.3.18)
γ q& = 2P2(q , q& )
R(&& && - DT ( MUBγ + S + QA ) = 0

v, v& , v) º DT MDv (5.3.23)
R &&v = D T MD R v& = - DT ( MUBγ q& + Sq& + QqA& ) D

ì- MUBP2 ( q,2Dv && - UBg ) + P4 ( q, BT U T ( MUBγ + S + Q A ) ) ü
ïï ïï (5.3.29)
R v = D T í+M 2 ( q, Dv && - UBγ ) - MUBg q - Sq - QqA ýD
ï ï
ïî+ ( MUBγ q& + Sq& + Qq& ) UBP2 ( q, q& )
A
ïþ
dR ¶v& ¶v
J trap = = R &&v + R v& n + R v n = R &&v + (h/2)R v& + (h 2 / 4) R v (5.3.43)
d&&
vn ¶&&
vn ¶&&
vn
dR
J RK = = R &&v + hgR v& + h 2 g 2 R v (5.3.52)
dk i
372
5.4 Numerical Examples with Tangent Space ODE
Four systems, one planar and three spatial, are simulated using the tangent space ODE
solution algorithms of Section 5.3. The effectiveness of constraint error control mechanisms that
are embedded in the formulation is verified. All functions that appear in the equations of motion
and derivatives required for numerical integration are explicitly derived. MATLAB computer
codes that implement the tangent space holonomic formulation and numerical solution for each
of the examples are provided in Appendix 5.A. Numerical integration is carried out with
Nystrom4, RK4,Kutta3/8, and RKFN explicit integrators and Trapezoidal and SDIRK54 implicit
integrators.
5.4.1 Planar Double Pendulum
The planar double pendulum shown in Fig. 5.4.1 is made up of a pair of uniform bars of
length two units each, moving in a vertical x-y plane, with gravity g acting in the negative y-
direction. Revolute joints at the origin of the x-y frame and at points (1,0) and ( - 1,0) in the
x1¢ -y1¢ and x¢2 -y¢2 body reference frames, respectively, define constraints. Generalized coordinates
for the system are
q º [ x1 y1 f1 f2 ]
T
x2 y2 (5.4.1)
For simplicity, inertia parameters are taken as m1 = m2 = 1 kg , J1¢ = J¢2 = 0.3 kg × m 2 , and
g = 9.8 m / sec2 . In this example, with centroidal body fixed reference frames, S ( q, q& ) = 0 .

Torsional springs and dampers act between ground and body one at the origin of the x-y
inertial frame, with spring and damping coefficients K1 and C1 , and between bodies one and two
at their common pivot point, with spring and damping coefficients K 2 and C2 . In order that
equilibrium is achieved with f1 = f2 = -p / 2 , the torque in the spring acting between ground and
body one is K1 (f1 + p / 2) and the torque in the spring between bodies one and two is
K 2 (f2 - f1 ) . To derive generalized forces that act on the system due to the springs, dampers, and
gravity, the virtual work done on the system by these forces is
373
δW= - gδy1 - gδy 2 + n1δf1 + n 2 δf2 - K1 (f1 + p / 2)δf1 - C1f& 1δf1
2 2 1 2 1 2 (
- K ( f - f )( δf - δf ) - C f& - f& ( δf - δf ) 2 1 ) 2 1
(
= -gδy1 - - n1 + K1 (f1 + p / 2) + C1f& 1 - K 2 ( f2 - f1 ) - C 2 f& 2 - f& 1 δf1 (5.4.2) ( ))
( (
- gδy 2 - - n 2 + K 2 ( f2 - f1 ) + C 2 f& 2 - f& 1 δf2 ))
º Q A δq
where n1 and n 2 are externally applied torques on bodies one and two. Thus,
é 0 ù
ê -g ú
ê ú
Q = êê
A
1(
ê - -n + K (f + p / 2) + C f& - K ( f - f ) - C f& - f&
1 1 1 1 2 2 1 2 2 1 ( )) ú
ú (5.4.3)
0 ú
ê ú
ê -g ú
ê ú
ëê (
- -n 2 + K 2 ( f2 - f1 ) + C2 f& 2 - f& 1 ( )) ûú
Constraint equations and their Jacobian are
é x1 - cos f1 ù
ê y1 - sin f1 ú
F (q) = ê ú =0 (5.4.4)
ê x1 + cos f1 - x 2 + cos f2 ú
ê ú
ë y1 + sin f1 - y 2 + sin f2 û
é1 0 sin f1 0 0 0 ù
ê0 1 - cos f1 0 0 0 úú
Fq ( q ) = ê (5.4.5)
ê1 0 - sin f1 -1 0 - sin f2 ú
ê ú
ë0 1 cos f1 0 -1 cos f2 û
The six nonzero terms in the 4 ´ 6 matrix P 2(q, c ) of Eq. (5.3.10) are
P2(q, χ )13 = cf1 cos f1 ; P 2(q, c)23 = cf1 sin f1; P 2(q, c)33 = -cf1 cos f1;
(5.4.6)
P2(q, χ )36 = cf2 cos f2 ; P 2(q, c)43 = cf1 sin f1; P2(q, c) 46 = cf2 sin f2 ;
These quantities are sufficient for explicit numerical integration.

For implicit numerical integration, the six nonzero terms in the 4 ´ 6 matrix P3(q, q& ) of
Eq. (5.3.17) are
P3(q, q& )13 = -f& 12 sin f1; P3(q, q& ) 23 = f& 12 cos f1; P3(q, q& )33 = f& 12 sin f1
(5.4.7)
P3(q, q& )34 = f& 22 sin f2 ; P3(q, q& )43 = -f& 12 cos f1 ; P3(q, q& )46 = -f& 22 cos f2
374
the two nonzero terms in the 6 ´ 6 matrix P 4(q, h) of Eq. (5.3.20) are
P4(q, η)33 = ( h1 - h3 ) cos f1 + ( h2 - h4 ) sin f1

(5.4.8)
P4(q, η)66 = - ( h3 + h4 ) sin f2
and the four nonzero terms in the 6 ´ 6 QqA and QqA& matrices are
QqA33 = -K1 - K 2 ; QqA36 = K 2 ; QqA63 = K 2 ; QqA66 = - K 2

(5.4.9)
QqA& 33 = -C1 - C 2 ; QqA& 36 = C2 ; QqA& 63 = C 2 ; QqA& 66 = -C 2
All other derivative terms are zero.

Initial conditions that satisfy position and velocity constraints are that both bodies hang
vertically downward at t 0 = 0 , q 0 = [ 0 -1 - pi/2 0 -3 - pi/2] with velocity
T
q& 0 = [0 0 0 10 0 10 ] ; i.e., body one at rest and angular velocity of body two is 10
T
rad/sec. Ten second simulations are carried out. The plot on the left of Fig. 5.4.2 with
K1 = K 2 = 20 N × m/rad , no damping, and no external torques was obtained with MATLAB Code
5.4.1 of Appendix 5.A, using the Nystrom4 explicit integrator with a constant step size of 0.001
sec. Total energy for this conservative system was constant to nine decimal places. Seventy nine
reparameterizations in 10,000 time steps (127 time steps per reparameterization) were required,
based on a unit limit on the norm of v. MATLAB timing results showed that less than one
percent of CPU time was required for reparameterization. The plot on the right of Fig. 5.4.2 was
obtained with the same data, except that damping was C1 = C2 = 10 N × m × sec/rad . Clearly,
energy is dissipated. Just 12 reparameterizations in 10,000 time steps (833 time steps per
reparameterization) were required, based on a unit limit on the norm of v. Comparable results
were obtained with all six integration methods.
(a) No Damping (b) Damped

Figure 5.4.2 Rotation of Body 2 vs. Time
As a check on constraint error control, the convergence tolerance Tol=utol=Btol in Eqs.
(5.2.17) and (5.2.20) was varied to evaluate its influence on constraint error. Results presented in
Table 5.4.1, obtained with the damped simulation on the right of Fig. 5.4.2, show that the
maximum norm of position, velocity, and acceleration errors over the simulation interval are
375
driven toward zero to computer precision, as convergence tolerances are tightened. Little increase
in compute time was associated with the tighter tolerances.
Table 5.4.1 Maximum Constraint Error
Tol Position Err. Velocity Err. Acceleration Err.
e-6 e-9 3e-11 3e-10
e-9 e-9 e-14 e-13
e-12 2e-14 3e-16 2e-15
5.4.2 Top with Tip Fixed

The symmetric Top that is presented as a special case to illustrate the tangent space
concept in Section 5.1 is treated here using the tangent space ODE formulation of Sections 5.2
and 5.3. Both the spin stabilized version studied in Section 5.1 and a transient dynamic variant
used in the literature as a test problem are analyzed, using MATLAB Codes 5.4.2a and 5.4.2b of
Appendix 5.A.
5.4.2.1 Spin Stabilized Top
The spin stabilized Top shown in Fig. 5.4.3 is constrained so that the tip is fixed at the
origin of the inertial reference frame. A centroidal body fixed x¢-y¢-z¢ frame, with z ¢ axis along
the axis of symmetry of the top and origin 1 m from the tip, is used to model the top, so
generalized coordinates include the vector r from the origin of the global x-y-z frame to origin of
the centroidal frame. As in Section 5.1, the top is to be stabilized by an initial angular velocity
omegaz0 about the body fixed z ¢ axis. The top is initially oriented so that body fixed and global
axes are aligned; i.e., r 0 = u z and p 0 = [1 0 0 0] . Initial angular velocity is
T
T
ω ¢0 = éë10 -12 10 -12 omegaz0 ùû rad/sec, where initial angular velocities of 10-12 about the body
fixed x ¢ and y¢ axes are perturbations to cause precession of the top relative to the global z axis.
% ¢0u¢z m/sec, where u¢z is the
Using Eq. (2.4.44), the initial velocity of the centroid is &r 0 = A(p0 )ω
unit vector along the positive z¢ axis. The mass of the top is 30 kg and the inertia matrix relative
to the centroidal x¢-y¢-z¢ frame is J¢ = diag(90, 90,30) kg × m2 . Note that the parallel axis
theorem of Section 4.4.2 has been used to transform the inertia matrix from the noncentroidal
frame used in Section 5.1 to the centroidal frame used here. A gravitational acceleration of 9.8
m / sec 2 acts in the negative global z direction.
376
Figure 5.4.3 Symmetric Top with Tip Fixed
Holonomic kinematic and Euler parameter normalization constraints on Cartesian
T
generalized coordinates q = éër T p T ùû are
é r - A(p)u¢z ù
F(q) = ê T ú=0 (5.4.10)
ë(p p - 1) / 2 û
Derivatives of kinematic constraints required for analysis are
éI -B(p,u¢z )ù
Φq = ê 3 ú
ë0 pT û
é0 - B(χ p ,u¢z )ù
P2(q, χ) º F q χˆ ( ) =ê ú
ëê0 χ pT ûú
q
(5.4.11)
& º F q q&ˆ q&ˆ
P3(q,q) (( ) q
) =0
q
é0 0 ù
(
P4(q, η) º F qT ηˆ ) =ê ú
ë0 - K(u¢z , η k ) + hp I 4 û
q
where B (p , u ¢z ) is given by Eq. (2.6.25) and K(a¢,b) is given by Eq. (2.6.37). Derived terms
from Eq. (5.3.18) are
γ = P 2(q, q& )q&
γ q = P 3(q, q& ) = 0 (5.4.12)
γ q& = 2P 2(q, q& )
With the centroidal reference frame,
377
é mI 3 0 ù
M(q) = ê ú
êë 0 4G ( p ) J¢G ( p ) úû
T
é 0 ù
& =ê
S(q,q) ú (5.4.13)
êë8G ( p& ) J ¢G ( p& ) p úû
T
é -mgu z ù
& t) = ê
Q A (q, q, ú
ë 0 û
where G(p) is given by Eq. (2.6.2). Kinetic derivatives that are required for analysis are
QqA = QqA& = 0 and
é0 0 ù
M 2(q, μ) º ( M(q)μˆ )q = ê ú
êë 0 -4G ( p ) J ¢G ( μ p ) + 4T ( J ¢G ( p ) μ p ) úû
T
é 0 ù
Sq = ê ú (5.4.14)
ëê8G ( p& ) J ¢G ( p& ) ûú
T
é 0 ù
Sq& = ê ú
ë -8G ( p& ) J ¢G ( p ) + 8T ( J ¢G ( p& ) p ) û
where μp is comprised of the last four components of the 7-vector μ and T(a) is given by Eq.
(2.6.20).
The residual form of the tangent space ODE of Eq. (5.3.1) is
&& - DT ( MUBg + S + QA ) = 0
R º DT MDv (5.4.15)
For implicit integration, derivatives of the residual given in Eq. (5.3.29) are
R &&v = DT MD
R v& = -DT ( MUBγ q& + Sq& + QAq& ) D (5.4.16)
ì-MUBP 2 ( q, 2Dv && - UBg ) + P4 ( q, B T U T (MUBγ + S + Q A ) ) ü
ïï ïï
R v = DT í+M 2 ( q, Dv && - UBγ ) - MUBg q - Sq - QqA ýD
ï ï
îï- ( MUBγ q& + Sq& + Qq& ) UB ( P 2 ( q, UBF t - Dv ) - F tq )
A
&
þï
where D = ( I - UBΦq ) V .
Six numerical integration algorithms of Section 4.8 are implemented in tangent space
ODE MATLAB Code 5.4.2a of Appendix 5.A, with data summarized here. Numerical
simulations with this formulation were carried out over a simulation interval of 100 sec, with
integration error tolerances Intol = Atol = e-6 for constant and variable step size integrators, an
iteration limit of 8 in implicit integration, and a bound of 0.75 on the norm of v. Results were
378
essentially identical to those shown in Fig. (5.1.6). Total energy for this conservative system was
constant to seven decimal places in simulations with omega0 = 13.5. For this top, gravitational
and constraint reaction forces at the tip create no torque about the z-axis, so the z¢ component of
angular momentum must be conserved (Arnold, 1989). The z¢ component of angular momentum
over the 100 sec simulation was constant to nine decimal places.
As a check on constraint error control, the convergence tolerance Tol = utol = Btol in
Eqs. (5.2.17) and (5.2.20) was varied to evaluate its influence on constraint error. The maximum
norms of constraint errors encountered with the trapezoidal integrator and omega0 = 13.5 over
the simulation interval are presented in Table 5.4.2. These results show that position, velocity,
and acceleration constraint errors are driven toward zero, to computer precision, as convergence
tolerances are tightened. An insignificant increase in compute time was associated with the
tighter tolerances.
Table 5.4.2 Maximum Constraint Error For Trapezoidal Integrator
e-6 e-6 2.5e-8 1.5e-7
e-9 e-9 3e-9 3e-8
e-12 e-12 e-14 1.4e-13
5.4.2.2 Transient Top

A test problem that is used in the literature (Bruls and Cardona, 2010; Terze, Muller,
and Zlatar, 2014) to evaluate formulations and numerical methods is the transient Top of Fig.
5.4.3, with gravitational acceleration 9.81 m / sec 2 acting in the negative y direction, a mass of 15
kg, and an inertia matrix J¢ = diag(0.234357,0.234357, 0.46875) kg × m2 . Initial angular velocity
is ω¢0 = [ 0 -4.61538 150] and the associated initial velocity is &r 0 = ω
% ¢0u z . Integration
T
accuracy, iteration limits, and v norm limits are as in Section 5.4.2.1. Simulations were carried
out with constant step size h = 0.0001, since transient motion of this top is severe.
The plot in Fig. 5.4.4 of the y coordinate of the centroid vs time, obtained with implicit
trapezoidal integration in MATLAB Code 5.4.2b of Appendix 5.A, is close to results presented
in the papers cited. The total energy of the system in this simulation is 5,583 N × m , with a
maximum variation of 0.018% over the 2 sec simulation reported here. This contrasts with
significant energy loss predicted by the second order generalized- a time integration method
used by Bruls and Cardona (2010), which imposes numerical damping to achieve stability of
their integrator. Constraint errors were similar to those cited in Table 5.4.2. Identical results were
obtained with explicit Nystrom4 and implicit SDIRK54 integration methods.
379
Figure 5.4.4 y-Coordinate of Centroid for Transient Top
5.4.3 Top with Tip Sliding on x-y Plane
A variation on the spin stabilized Top example of Sections 5.1 and 5.4.2.1 is to relax the
constraint on the tip T, allowing it to move without friction in the x-y plane, as shown in Fig.
5.4.5. A spring-damper with coefficients K and C acts between the tip T and the origin O of the
T
x-y plane. Generalized coordinates are q = éër T p T ùû , where r is the vector from the origin of
the x-y-z inertial frame in Fig. 5.4.5 to the centroid of the Top that is a unit distance from the tip.
With the unit vector u z = [ 0 0 1] in the positive z-direction, holonomic constraint equations and
T
their Jacobian are

éuT ( r - A (p)u¢z ) ù
F (q) = ê z ú=0 (5.4.17)
ë 1/ 2( p T
p - 1) û
éu T -uTz B(p,u¢z ) ù
F q (q) = ê z ú (5.4.18)
ë0 pT û
This Top has five degrees of freedom, whereas the top with tip fixed has only three degrees of
freedom.
Figure 5.4.5 Top with Tip Sliding on the x-y Plane
380
The variational equation of motion of Eq. (4.3.24) for the top, with its centroidal body
reference frame, is
r + δp T ëé4G(p )T J ¢G(p )p
mδr T&& && - 8G(p& )T J ¢G(p& )p ûù - dq TQ A = 0 (5.4.19)
which must hold for all δr and δp that satisfy F q dq = 0 . Terms in Eq. (4.3.28) are
é mI 0 ù
M (q) = ê 3 ú
ë 0 4G (p) J ¢G (p) û
T
(5.4.20)
é 0 ù
S(q, q& ) = ê ú
ë8G (p& ) J¢G (p& )p û
T
With a spring force F = - Kr tip - Cr& tip that acts on the tip of the top at point
r tip = r - A (p)u¢z and gravity acting in the negative z-direction, the virtual work of forces on the
Top is
δW = - mgdr T u z - Kδr tipT r tip - Cδr tipT r& tip
= - mgdr T u z - K ( δr - B (p, u¢z )δp ) ( r - A (p )k ¢ )
T
- C ( δr - B (p, u¢z )δp ) ( r& - B(p , u¢z )p& )

T
é - mgu z - K ( r - A (p )u ¢z ) - C ( r& - B (p, u ¢z )p& ) ù

= éë δr T δp T ùû ê ú º éë δr
T
δp T ùû Q A
ë B ( p , u ¢
z ) T
( (
K r - A ( p )u z)
¢ + C ( &
r - B (p , u ¢
z )p& ) )û
Thus, the applied generalized force is
é -mgu z - K ( r - A(p)u¢z ) - C ( r& - B(p, u¢z )p& ) ù
QA = ê ú
ëB(p, u¢z ) ( K ( r - A(p)u¢z ) + C ( r& - B(p, u¢z )p& ) ) û
T (5.4.21)
For implicit numerical integration, terms from Eqs. (5.3.7), (5.3.10), (5.3.17), (5.3.20),
(5.3.25), (5.3.40), and (5.3.41) are
381
D = (I - UBΦq )V
é 0 -u Tz B(χ p , u¢z ) ù
P 2(q, χ ) = ( Φq χˆ ) =ê ú
ëê 0 χ pT ûú
q
(
P3(q, q& ) = P 2(q, q&ˆ )q&ˆ ) q
=0
é0 0 ù
P 4(q, η) = ( Φ qT ηˆ ) = ê ú
ë 0 -h1K (u z , u¢z ) + h2 I 4 û
q
é0 0 ù
M 2(q, μ ) = ( Mμˆ )q = ê ú
ë 0 4T(J¢G (p)μ p ) - 4G (p) J ¢G (μ p ) û
T
é - KI 3 KB(p, u¢z ) + CB(p& , u¢z ) ù

QqA = ê ú
ë KB (p, u¢z ) a
T
û
a = K (u¢z , ( K(r - A(p)u¢z ) + C(r& - B(p, u¢z )p& ) ) - B(p, u¢z )T ( KB(p, u¢z ) + CB(p& , u¢z ) )
é -I 3 B(p, u¢z ) ù
QqA& = C ê ú
ë B(p, u¢z ) -B(p, u¢z ) B(p, u¢z ) û
T T
é0 0 ù
Sq = ê ú
ë 0 8G (p& ) J¢G (p& ) û
T
(5.4.22)
é0 0 ù
Sq& = ê ú
ë 0 8T(J¢G (p& )p) - 8G (p& ) J¢G (p) û
T
Since the holonomic constraint of Eq. (5.4.17) does not depend explicitly on t and P3(q, q& ) = 0 ,
γ = P2(q, q& )q&
γq = 0 (5.4.23)
γ q& = 2P 2(q, q& )
Inertia properties are m = 30 kg, J¢ = diag(90,90,30) kg × m2 relative to the centroidal

x¢-y¢-z¢ frame, and g = 9.8 m/sec 2 . The tops of this and Sections 5.1 and 5.4.2.1 are identical,
with the exception that here a centroidal inertia matrix is used, whereas in Section 5.1 the inertia
matrix is with respect to the tip. The initial configuration is with the top vertically upward;
r 0 = [ 0 0 1] and p 0 = [1 0 0 0] . The initial angular velocity in the body fixed reference
T T
frame is w¢0 = [ e e omegaz0] , where x¢ and y¢ angular velocities ε = 10-12 are perturbations
T
from the vertical. Initial generalized coordinate velocities are r& 0 = 0 and p& 0 = 0.5G (p 0 )T w¢0 .
Total energy TE = 2p& TG(p )T J ¢G(p )p& + mgz should be constant for this conservative system.
The plots in Fig. 5.4.6 were obtained with MATLAB Code 5.4.3 of Appendix 5.A, using
the trapezoidal implicit second order integrator with h = 0.001 sec and K = C = 0 . The 40 sec
382
simulations reported show tx and ty coordinates of the tip, for simulations with three initial
values omegaz0 of z ¢ -angular velocity. As shown in the fourth plot at the lower right, the
horizontal displacement of the centroid is zero to computer precision, unlike the Top with tip
fixed in Sections 5.1 and 5.4.2.1, for which the centroid moves with significant amplitude. The
plots in Fig. 5.4.6 indicate that the top stabilizes at approximately the same angular velocity as
the Top with tip fixed. Total energy and angular momentum are constant to seven decimal places
in each simulation and less than 200 reparameterizations in 40,000 time steps were required
(200 time steps per reparameterization), all due to a limit of 50 on condition number of the
reduced mass matrix. Less than one percent of computation effort was utilized for
reparameterization.
Figure 5.4.6 tx-ty Trajectories for Tip and x-y Trajectory of Centroid, K = C = 0
Simulations carried out with the four explicit integrators of Section 4.8 yielded
essentially identical results as those reported. Simulations carried out with the implicit SDIRK54
algorithm of Section 4.8 yielded the same results, but required somewhat greater CPU time.
To investigate the influence of a nonzero spring coefficient K on motion of the Top,
simulations over a 40 sec period were carried out with K = 50 N/m (a stiffness characteristic of a
rubber band acting on the tip of the 30 kg Top), C = 0, and initial angular velocities shown in
383
Fig. 5.4.7. The horizontal trajectories in Fig. 5.4.7 show a marked difference in character from
those of the top with K = 0. The trajectories on the left are displacement of the tip and those on
the right are displacement of the centroid. Unlike the case with K = 0 and zero centroidal
displacement shown at the lower right of Fig. 5.4.6, with the nonzero spring constant the centroid
and tip move with similar radii in horizontal planes. It is interesting that the top with spring
attached to the moving tip requires a significantly higher initial vertical angular velocity to
achieve stable motion. As noted above, all six integration algorithms give essentially identical
results.
Figure 5.4.7 tx-ty and x-y Trajectories of Tip and Centroid, K = 50, C = 0
As a numerical experiment that demonstrates favorable attributes of implicit integration
algorithms, very large values of K and C are used, to simulate the Top with fixed tip studied in
Sections 5.1 and 5.4.2.1. To be consistent with initial conditions for the Top with tip fixed, the
T
same initial configuration was used and initial velocities are ω¢ = éë10- 10- omegapr0ùû
0 12 12
% ¢0u z . With K = C = 106 , the three fixed step size explicit algorithms (Nystrom4,
and &r 0 = ω
Runge-Kutta4, and Kutta3/8) failed during the first few time steps, as expected due to stiffness
384
associated with large physical stiffness and damping in this example. The variable step size
RKFN54 explicit algorithm with tight absolute error tolerance Atol was able to reduce step size
and calculate a numerical solution, but the results did not comport with reality. Simulations
carried out with the implicit constant step trapezoidal and variable step SDIRK54 algorithms
provide meaningful solutions. Results presented in Fig. 5.4.8, using the implicit trapezoidal
algorithm and omegaz0 = 13.5, indicate that implicit algorithms can indeed handle stiff problems
and yield results close to those in Fig. 5.1.3. While the tip displacement at the right of Fig. 5.4.8
shows high frequency activity, its magnitude is zero to computer precision. The trajectory at the
left of Fig. 5.4.8 is remarkably similar to the plot at the lower right of Fig 5.1.3, for the same
input data.
Figure 5.4.8 Simulation of Top with Tip Fixed, Using K = C = 106

Eqs. (5.2.17) and (5.2.20) is varied to evaluate its influence on constraint error in the simulation
at the upper right of Fig. 5.4.6. Results presented in Table 5.4.3 show that the maximum norm of
position, velocity, and acceleration errors over the simulation interval are driven toward zero to
computer precision, as convergence tolerances are tightened. Little increase in compute time is
associated with the tighter tolerances.
e-6 e-6 6e-7 3e-5
e-9 e-9 2.5e-9 1.5e-8
e-12 e-12 8e-13 3e-11
5.4.4 Spatial Double Pendulum

The spatial double pendulum of Section 3.3.7.1 is made up of two spheres of unit radius,
as shown in Fig. 5.4.9. The origin of body 1 is fixed to the origin of the global x-y-z frame,
permitting the body to rotate freely about that point. A unit distance constraint acts between
385
points P1 on body 1 at -u1z¢ and P2 on body 2 at u¢2z , where u1z
¢ and u¢2z are unit vectors along
the positive body fixed z1¢ and z¢2 axes. The mass of each sphere is m = 75 kg, the inertia matrix
of each sphere is J¢ = (2m/5)I kg × m2 , and a gravitational acceleration of g = 9.8 m / sec2 acts in
the negative z-direction. Euler parameter and position initial conditions are
p10 = p02 = [1 0 0 0] and r20 = [ 0 0 -3] ; i.e., the body z ¢ axes coincide with the inertial z
T T
axis and body two hangs vertically downward. The initial velocity of body two is r&2 = 0 and
initial angular velocities are w1¢0 = [ Omeg1 0 0] and w¢20 = [0 Omeg2 0] , so
T T
p& 0i = 0.5G(p0i )T w¢i 0 , i = 1,2 .
Figure 5.4.9 Spatial Double Pendulum

T
Generalized coordinates for this system are q = éëp1T p T2 r2T ùû Î R 11 . The vector r 21
from point P2 to P1 is
r 21 = r2 + A(p 2 )u¢2z + A(p1 )u1z

¢ (5.4.24)
Holonomic constraints and their required derivatives are
é 1/ 2(p1Tp1 - 1) ù
ê ú
F(q) = ê 1/ 2(pT2 p 2 - 1) ú = 0 (5.4.25)
ê1 / 2(r 21Tr 21 - 1) ú
ë û
é p1T 0 0 ù
ê ú
F q (q ) = ê 0 p2T
0 ú
ëêr B(p1 , u1z
¢ ) r 21T B(p2 , u¢2z ) r 21T ûú
21T
386
é χ Tp1 0 0ù
ê ú
P2(q, χ ) = ê 0 χ p2
T
0ú
êa T B(p1 , u1z ¢ ) a T B(p 2 , u¢2z ) + r 21T B( χ p2 , u¢2z ) aT úû
¢ ) + r 21T B( χ p1 , u1z
ë
where cp1 , cp2 , and cr2 are components of χ Î R11 that correspond to p1 , p 2 , and r2 and
¢ )χ p1 + B(p 2 , u¢2z )χ p2 + χ r2 ,
a = B(p1 , u1z
é0 0 0ù
P3(q, q& ) = êê 0 0 0 úú
êëb1 b 2 b3 úû
¢ )p& 1 + B(p2 , u¢2z )p& 2 + r&2 ,

¢ ) + p& T2 BT (p2 , u¢2z ) + r&2T , e = B(p1 , u1z
where c = p& 1T BT (p1 , u1z
¢ ) + p& T2 BT (p& 2 , u¢2z ) , b1 = ( eT + c ) B(p& 1 , u1z
d = p& 1T BT (p& 1 , u1z ¢ ) + dB(p1 , u1z
¢ ),
b 2 = ( e T + c ) B (p& 2 , u¢2z ) + dB (p 2 , u¢2z ) , and b 3 = d ,
é c1 ¢ )B (p 2 , u¢2z ) h3B T (p1 , u1z
h3B T (p1 , u1z ¢ )ù
ê ú
P 4(q, η) = ( F qT ηˆ ) = ê h3B (p 2 , u¢2z )B(p1 , u1z
T
¢) c2 h3 B (p 2 , u¢2z ) ú
T
q
ê h3B (p1 , u1z¢ ) h3B (p 2 , u¢2z ) h3I 3 ú
ë û
¢ ,r 21 ) + h3BT (p1 ,u1z
where c1 = h1I 4 + h3K(u1z ¢ )B(p1 ,u1z
¢ ) and
c 2 = h2I 4 + h3K(u¢2z ,r 21 ) + h3B T (p 2 ,u¢2z )B(p 2 , u¢2z ) . Finally, from Eq. (5.3.18),
& &
g = P 2(q,q)q
&
g q = P3(q,q)
&
g&q = 2P 2(q,q)
The variational equation of motion for the system is obtained by summing variational
equations for each of the bodies,
2
mδr2T [&&
r2 + gk ] + å δpTi éë4GT (p i )J ¢G(pi )&&
pi - 8GT (p& i )J ¢G(p& i )pi ùû = 0
i =1
which must hold for all dq such that F q dq = 0 . The mass matrix, velocity coupling vector, and
generalized force vector are
é(8 / 5)G T (p1 )G(p1 ) 0 0ù
ê ú
M( q) = m ê 0 (8 / 5)G (p 2 )G(p 2 ) 0 ú
T
êë 0 0 I 3 úû
387
é GT (p& 1 )G(p& 1 )p1 ù
16m ê T ú
S(q, q& ) = ê G (p& 2 )G(p& 2 )p2 ú
5
êë 0 úû
T
Q A = éë 0 0 - mgu Tz ùû
Derivatives of kinetic quantities for implicit integration are QqA = Q&qA = 0 and
éG T (p
& 1 )G(p
& 1) 0 0ù
16m ê
Sq = ê 0 G (p
T
& 2 ) 0 úú
& 2 )G(p
5
ê 0 0 0 úû
ë
& 1 )G(p1 ) + T ( G(p
é -GT (p & 1 )p1 ) 0 0ù
16m ê
S&q = ê 0 -G (p
T
& 2 )p 2 ) 0 úú
& 2 )G(p2 ) + T ( G(p
5
ê 0 0 0 úû
ë
ê 1
(
é -G T (p1 )G(μ p ) + T G(p1 )μ p
1
) 0 0ù
ú
M 2(q,μ) =
8m ê
5 ê
0 (
-G T (p 2 )G(μ p2 ) + T G(p 2 )μ p2 ) 0ú
ú
ê 0 0 0ú
ë û
where μp1 and μp 2 are components of μ Î R11 that correspond to p1 and p 2 .
The plot in Fig 5.4.10 is obtained using MATLAB Code 5.4.4 of Appendix 5.A, with the
Nystrom4 explicit second order integrator of Section 4.8. It shows the x-value of the centroid vs
time for body 2, for Omeg10 = 0 and Omeg20 = 5 rad/sec. Total energy is constant to eight
decimal places for this conservative system and only one parameterization in 20,000 time steps
was required. Constraint reaction force in the massless bar that connects the bodies, in Newtons,
calculated using the third component of the Lagrange multiplier of Eq. (5.2.54), which
corresponds to the third constraint equation of Eq. (5.4.25), is shown at the left of Fig. 5.4.11. To
see the effect of reducing the severity of input, constraint reaction force for input Omeg2 = 2.5 is
shown at the right of Fig. 5.4.11.
388
Figure 5.4.10 x-Coordinate of Centroid of Body 2 vs Time
Figure 5.4.11 Constraint Reaction Force in the Bar Between Points P1 and P2 vs Time
Eqs. (5.2.19) and (5.2.22) is varied to evaluate its influence on constraint error in the simulation
of Fig. 5.4.10. Results presented in Table 5.4.4 show that the maximum norm of position,
velocity, and acceleration errors over the simulation interval are driven toward zero to computer
precision as the convergence tolerance is tightened. Little increase in compute time is associated
with the tighter tolerances.
e-6 e-6 e-7 e-6
e-9 e-9 e-12 e-11
e-12 e-12 e-13 e-12
389
5.4.5 Need for Automated Equation Formulation and Solution
While accurate solution of multibody system dynamics with the tangent space ODE
formulation is demonstrated, via ad-hoc derivation of governing equations and ad-hoc
programming of numerical solution methods, a great deal of person time and effort is required.
Motivation is thus provided for effort in later sections of the chapter to automate the formulation
and solution process. This topic is addressed in Sections 5.7 through 5.10.
Numerical examples presented using MATLAB codes in Appendix 5.A illustrate the
effectiveness of the tangent space ODE formulation and provide confidence that computer
implementation for more realistic applications is possible. Examples show that the tangent
space parameterization is efficiently updated, based on criteria provided, and yields a unique
solution throughout the time interval of interest. Parametric excursions show that all three
forms of kinematic constraint are satisfied to specified accuracy, a property that very few
computational approaches in mechanical system dynamics achieve. Finally, the third example
shows that implicit numerical integration of tangent space ODE for a stiff model yields
accurate results, which is not possible with explicit integration methods. This is probably the
best definition of a stiff system.
390
5.5 Tangent Space Index 0 DAE
The ODE of Eq. (5.2.48) is transformed to an equivalent Index 0 form of DAE that
introduces Lagrange multipliers as the derivative of a variable that is determined in numerical
integration. This formulation is thus applicable for systems with friction, since the Lagrange
multipliers may be used to define constraint reaction forces and associated friction forces. Just
as in the ODE formulation of Section 5.3, the Index 0 formulation is based on the tangent space
kinematics of Section 5.2 that includes embedded tolerances to assure satisfaction of
configuration, velocity, and acceleration constraints. Explicit and implicit numerical integration
formulas are applied to the underlying ODE and inflated to the Index 0 DAE form for numerical
solution. As shown in Section 5.11, Index 0 DAE inherit attractive theoretical properties
associated with ODE integration.
5.5.1 Index 0 DAE Formulation
Holonomic constraints are of the form
F (q, t) º Φ(q ) - f (t) = 0 Î R nhc (5.5.1)
in generalized coordinates q Î R ngc , including Euler parameter normalization conditions for

spatial bodies. In addition to satisfying the constraints of Eq. (5.5.1), q and its derivatives must
satisfy velocity and acceleration constraint equations that are obtained by differentiating Eq.
(5.5.1) with respect to time,
F q q& = ft º ν (5.5.2)
(( )
&& = - F qq&ˆ q& - f tt º - γ
F qq
q
) (5.5.3)
Using the tangent space kinematics formulation of Section 5.2, the ODE of Eq. (5.2.48)
that governs system dynamics is
&& = DT ( MUBg + S + QA )
DT MDv (5.5.4)
with initial conditions of Eq. (5.2.51),

v(t 0 ) = 0
(5.5.5)
v& (t 0 ) = V T q& 0
Using the definition D(q ) º ( I - UBΦ q ) V in Eq. (5.2.24), Eq. (5.5.4) may be written in
the form
{ && + F Tq BT U T ( MUBg + S + QA ) - ( MUBg + S + QA )

&& - F Tq BT U T MDv
V T MDv }
{ }
(5.5.6)
&& + F qT BT U T é( MUBg + S + QA ) - MDv
= V MDv
T
&& ù - ( MUBg + S + Q A ) = 0
ë û
Defining
μ& = BT U T éë( MUBg + S + QA ) - MDv

&& ù
û (5.5.7)
μ(t 0 ) = 0
391
Eq. (5.5.6) becomes
{ }
&& + F qTμ& - ( MUBg + S + QA ) = 0
V T MDv (5.5.8)
Taking the transpose of Eq. (5.2.13), written in the form BΦq U = I , yields the identity
U T F qT B T = I . Multiplying the first of Eqs. (5.5.7) on the left by the nonsingular matrix U T F Tq
yields
UTFqTμ& = U TF Tq BT UT ( MUBg + S + QA ) - U TF qT BT U T MDv
&&
With the foregoing identity, this is
{ }
&& + FqTm& - ( MUBg + S + Q A ) = 0
U T MDv (5.5.9)
Combining Eqs. (5.5.8) and (5.5.9),

é VT ù
(
&& + F Tq μ& - ( MUBg + S + Q A ) = 0
ê T ú MDv
ëU û
)
éVT ù
Since the columns of V and U span R ngc , the matrix ê T ú = [ V U ] is nonsingular and
T
ëU û
MDv && + F Tq μ& - MUBg - S - Q A = 0 (5.5.10)
If, instead of defining the term on the right of Eqs. (5.5.7) as m& it were defined as l , Eq.
(5.5.10) would be
MDv && + F qT λ - MUBg - S - Q A = 0 (5.5.11)
which has been called an Index 0 DAE (Haug, 2017b), because it is a DAE that reduces to the
ODE of Eq. (5.5.10) with no differentiations (Ascher and Petzold, 1998) by simply defining
l = m& . Equation (5.5.11) is thus called an Index 0 DAE.
The physical significance of the vector m in Eq. (5.5.7) is not clear, so assignment of the
initial condition of Eq. (5.5.7) may be of no consequence, other than assuring there is a unique
solution of the initial-value problem.
The steps from Eq. (5.5.4) in deriving Eqs. (5.5.10) and (5.5.11) are reversible, so Eq.
(5.5.11) is equivalent to the ODE of Eq. (5.5.4). The ODE theory summarized in Section 4.7.3
shows that the initial value problem of Eq. (5.5.4) with the associated initial values is well posed.
Since the initial-value problems of Eqs. (5.5.4) and (5.5.10), with the same initial conditions, are
equivalent, the Index 0 DAE with these initial conditions is also well posed.
Writing Eq. (5.5.10) in matrix form,
é &&
vù
éë MD F Tq ùû ê ú = MUBg + S + Q A (5.5.12)
ëμ& û
it is clear that for existence of a unique solution, the coefficient matrix on the left must be
nonsingular. At a parameterization point (q0 , t 0 ) that satisfies Eq. (5.5.1), Eqs. (5.2.6) and
392
(5.2.24) show that D0 = V . To show that the coefficient matrix on the left of Eq. (5.5.12) at
(q0 , t 0 ) , with D0 = V and F T0
q = U , is nonsingular, set
éα ù
[ MV U ] ê ú = MVα + Uβ = 0 (5.5.13)
ëβ û
If it can be shown that this equation holds only for α = 0 and β = 0 , then the coefficient matrix at
(q0 , t 0 ) is nonsingular. Multiplying Eq. (5.5.13) on the left by V T ,
VT MVα + V T Uβ = V T MVα = 0
Even though M is often singular, it is shown in Section 4.6.2 to be positive definite on the null
space of Fq ; i.e., V T MV is positive definite, hence nonsingular. Thus, α = 0 and, from Eq.
(5.5.13), Ub = 0 . Since U has full rank, β = 0 . This shows that the coefficient matrix on the left
of Eq. (5.5.13) is nonsingular. The coefficient matrix of Eq. (5.5.12) is thus nonsingular at
(q0 , t 0 ) and, since functions in the matrix are continuous in q and t, the coefficient matrix of Eq.
(5.5.10) is nonsingular in a neighborhood of (q0 , t 0 ) .
Finally, since Eq. (5.5.10) involves && v and m& , it is neither a first or second order ODE.
Defining y1 = v, y 2 = v& , and y3 = m Eq. (5.5.10) may be written as an equivalent system of first
order ODE in y = [ y1 y3 ] ,
T
y2
éI 0 0 ù é y2 ù
ê 0 MD F T ú y& = ê MUBg + S + Q A ú (5.5.14)
ë q û ë û
As in the case of the coefficient matrix in Eq. (5.5.12) the coefficient matrix of Eq. (5.5.14) is
nonsingular. The system of Eq. (5.5.14) with initial conditions from Eqs.(5.5.5) and (5.5.7),
T
y (t 0 ) = éë 0 q& 0T V 0 ùû (5.5.15)
is a first order initial-value problem that can be numerically integrated using the methods of
Section 4.8.
5.5.2 Explicit Integration of Index 0 DAE
Explicit integration of the Index 0 DAE initial-value problem of Eqs. (5.5.11) and Eq.
(5.5.5), using explicit Runge-Kutta methods, is as follows:
velocity constraints of Eqs. (5.5.1) and (5.5.2). Evaluate the constraint Jacobian F q (q 0 , t 0 )
and matrices U and V of Eqs. (5.2.5) and (5.2.6) and initial conditions of Eq. (5.5.5).
Evaluate B 0 of Eq. (5.2.13) at t 0 and updated at t using Eq. (5.2.20). Define u = h ( v, t) of
Eq. (5.2.14), and q of Eq. (5.2.8). Evaluate D of Eq. (5.2.24), and q& of Eq. (5.2.34).
Evaluate g of Eq. (5.5.3).
393
(2) Solve Eq. (5.5.11) by factoring éëMD FTq ùû at time step t i to determine && vi and λ i .
Apply an explicit numerical integrator to &v&i on a time grid with step size h to determine
vi +1 and v& i +1 . This determines v, v& , and &&v on the time grid. Use Eqs. (5.2.33) through
(5.2.35) to evaluate q, q& , and q && on the time grid.
(3) Monitor the condition number of éëMD FqT ùû , the norm of v, and the number of
iterations required to evaluate u and B. If tolerances are exceeded, define a new time t 0 and
associated q 0 and q& 0 . Repeat calculations in Step (1) to define a new parameterization and
initial conditions v 0 and v& 0 . This process follows the trajectory shown in Fig. 5.2.2, moving
smoothly across charts on the constraint manifold.
While Lagrange multipliers are not used in application of the explicit integrator, they are
available to determine constraint reaction forces in the system, using results of Section 4.10.
5.5.3 Derivatives for Implicit Integration of Index 0 DAE
To use an implicit numerical integration method to solve Eqs. (5.5.11) and (5.5.5),
derivatives of all terms that appear with respect to v and v& are required. These terms are actually
a simplified form of those appearing in the ODE of Section 5.3, so the needed derivatives may be
obtained using relations derived in Section 5.3
If Eq. (5.5.11) is to be solved numerically with an implicit numerical integration method,
a convenient form is the residual equation
R º MDv
&& + F qT λ - MUBg - S - Q A = 0 (5.5.16)
This form of the equations of motion in terms of &&v , l , q (v, t) , and &q(v,&v, t) is used to
iteratively solve a discretized form of the equation, based on implicit numerical integration
formulas. To implement this approach, expressions for derivatives of R with respect to
v, v& , and &&
v are required.
Expanding the term MDv && = MVv&& - MUBΦq Vv && , using Eq. (5.3.11), the chain rule of
differentiation can be used to obtain derivatives of the residual R of Eq. (5.5.16) with respect to
v, &v, and&&v . Using Eqs. (5.3.7), (5.3.14), (5.3.9), and (5.3.8),
( &&ˆ - MUBF q Vv
R v = MVv &&ˆ - MUBg + F qTlˆ - S - Q A )D
q
(
+ MUBg + S + Q A ) &q
&
UBP2(q,q)D
(5.5.17)
(
R&v = - MUBg + S + Q A
)
&q
D
R&&v = MD
Derivatives called for in Eqs. (5.5.17), evaluated using Eqs. (5.3.25), (5.3.12), (5.3.10), and
(5.3.20) are
394
( MVv&&ˆ ) = M2(q, Vv)
q
&&
( MUBF Vv&&ˆ ) = ( MUB

q
q
ˆ ˆFˆ Vvˆ&&ˆ ) + ( MUB
q
ˆ ˆ F ˆ&&ˆ ) + ( MUB
ˆ Vv
q
ˆ&&ˆ )
ˆ ˆ ˆ F Vv
q
q
q
q
&& - MUBP2(q, UBF q Vv)

= M 2(q, UBF q Vv) && + MUBP2(q, Vv)
&&
( MUBg )q = ( MUB
ˆ ˆ gˆ ) + ( MUB
q
ˆ ˆ gˆ ) + ( MUB
q
ˆ ˆ ˆg)
q
= M 2(q, UBg ) - MUBP2(q, UBg ) + MUBg q
(F lˆ )
T
q
q
= P4(q, l )
( MUB
ˆ ˆ ˆg)
&q
= MUBg&q
Using these relations, the fact that M 2(q, b ) and P 2(q , χ ) of Eqs. (5.3.25) and (5.3.10) are linear
in their second arguments, and Eqs. (5.3.34) and (5.3.35), Eqs. (5.5.17) become
&& - MUBP2(q,q)
é M 2(q,q) && - MUBg q + P4(q, l ) ù
Rv = ê úD
( &
êë -Sq - Q q + MUBg &q + S&q + Q&q UBP2(q,q)
A A
úû )
(
R&v = - MUBg &q + S&q + Q&qA D ) (5.5.18)
R&&v = MD
Finally, from Eq. (5.5.16),
¶R / ¶l = F qT (5.5.19)
5.5.4 Implicit Trapezoidal Integration of Index 0 DAE

Implicit trapezoidal integration formulas of Eqs. (4.8.40) are
v n = v n-1 + hv& n-1 + (h 2 / 4)(&&
v n -1 + &&
vn )
(5.5.20)
v& n = v& n-1 + (h/2)( &&
v n -1 + &&
vn )
These integration formulas are technically valid only for use with ODE. Without justification,
which follows in Section 5.5.5, substituting Eqs. (5.5.20) into Eq. (5.5.16), the residual R n is a
function of &v&n and the chain rule of differentiation yields the derivative of R with respect to &v&n ,
¶v& n ¶v
¶R / ¶&&
v n = R &&v + R v& + R v n = R &&v + (h/2)R v& + (h 2 / 4) R v (5.5.21)
¶&&
vn ¶&&
vn
v n -1 , and l n -1 . Thus, the trapezoidal Jacobian of the
with all arguments evaluated at v n -1 , v& n -1 , &&
residual of R of Eq. (5.5.16) evaluated at t n is
J trap
n = éë¶R / ¶&&v n Φ Tq ùû (5.5.22)
395
With initial conditions q(t 0 ) = q0 and q& (t 0 ) = q& 0 that satisfy F(q 0 , t 0 ) = 0 and
F q (q0 ,t 0 )q& 0 = ft (t 0 ) , initial values v0 = 0 and v& 0 = VTq& 0 are obtained from Eq. (5.3.2). To
v 0 and l 0 , which is needed to begin iterative solution of Eq. (5.5.16), Eq.
obtain an estimate of &&
(5.5.12) may be solved at t 0 with the initial conditions.
v n and l n , using the Jacobian of Eq. (5.5.22),
To carry out Newton-Raphson iteration for &&
éV&&
v in ù
J trap
n ê i ú
v in , l in )
= -R (&&
ëVl n û
i=1,2,L until R £ intol (5.5.23)
é &&
v i+1 ù é &&
v i
ù é V &&
v i
ù
ê i+1 ú = ê i ú + ê i ú
n n n
ël n û ël n û ëVl n û
is formed and solved until convergence is achieved. To evaluate the right side of the first of Eqs.
(5.5.23), Eqs. (5.5.20) are used to obtain v in and v& in , Eq. (5.2.19) is used to determine uin , Eq.
(5.2.20) is used to determine Bn , and Eqs. (5.2.33) and (5.2.34) are used to determine qin and q& in .
Since a precise Jacobian is not required to achieve convergence in Eq. (5.5.23), the integration
Jacobian J trap
n of Eq. (5.5.22) is held constant over iterations in Eq. (5.5.23).
5.5.5 Implicit Runge-Kutta Integration of Index 0 DAE
v = g ( t, v, v& ) , the Runge-Kutta integrator of
For a general second order ODE of the form &&
Eqs. (4.8.27) and (4.8.28) is
æ i i ö
k i = g ç t n -1 + ci h,v n -1 + hci v& n -1 + h 2 å A ijk j , v& n -1 + h å a i jk j ÷ , i=1L s
è j=1 j=1 ø
s
v n = v n -1 + hv& n -1 + h 2 å B jk j (5.5.24)
j=1
s
v& n = v& n -1 + h å b jk j
j=1
Applying this formula to the conventional ODE reformulation of Eq. (5.5.4),

v = M -1F
&& (5.5.25)
where M º D T MD and F º D T ( MUBg + S + Q A ) ,
( (
k i = M -1F (5.5.26)
(
The over-score notation f for a function or matrix f ( t , v, &v) is defined as
( æ i i ö
f (t, v, v& ) º f ç t n -1 + ci h,v n -1 + hci v& n -1 + h 2 å A ijk j , v& n -1 + h å a i jk j ÷ (5.5.27)
è j=1 j=1 ø
(
Multiplying both sides of Eq. (5.5.26) on the left by M ,
396
( (
Mk i = F (5.5.28)
Expanding this equation, using the definitions of M and F, yields

( ( ( ( ( ( ( ( ( (
V T MDk i - V T F qT B T U T MDk i + V T F Tq B T UQ - V T Q = 0 (5.5.29)
Defining
( ( ( ( (
l i º B T UQ - BT UMDk i (5.5.30)
Eq. (5.5.29) becomes
(( ( (
(
V T MDk i + F qT l i - Q i = 0) (5.5.31)
(
Multiplying both sides of Eq. (5.5.30) on the left by U T F Tq and using B T = ( U T F qT ) from Eq.
-1
(5.2.13) yields
( ( ( (
(
U T MDk i + F qT l i - Q i = 0) (5.5.32)
Since the columns of U and V span R n , their common coefficient in Eqs. (5.5.31) and (5.5.32)
must be zero; i.e.,
( ( ( (
R n º MDk i + F Tq l i - Qi = 0 (5.5.33)
This system of nonlinear algebraic equations may be solved using the Newton-Raphson method.
Partial derivatives of Eqs. (5.5.18) and the chain rule of differentiation with the arguments of Eq.
(5.5.27) and the second and third of Eqs. (5.5.24) yield
¶R n / ¶k i = MD + ha ii R v& + h 2 A ii R v (5.5.34)
For the SDIRK54 algorithm of Eqs. (5.5.24) and (4.8.43), a ii = 1/ 4 and Aii = a ii2 = 1/16 . Thus,
¶R n / ¶k i = R &&v + 0.25hR v& + 0.0625h 2 R v (5.5.35)

(
and, with ¶R / ¶l i = F Tq , the SDIRK54 integration Jacobian is
n ºé
J RK ë¶ R n / ¶ k i F qT ùû (5.5.36)
The iterative solution algorithm at stage j is thus

éVk ij ù
J ê j ú = -R (k i , l i )
RK j j
n
Vl
ë iû
j=1,2,L until R £ intol (5.5.37)
ék ij+1 ù ék ij ù éVk ij ù
ê j+1 ú = ê j ú + ê j ú
ë l i û ë l i û ëVl i û
for stages i = 1, L ,s. The solution v n and v& n is given by the second and third of Eqs. (5.5.24).
This procedure may be viewed as applying the Runge-Kutta algorithm to the DAE of Eq.
(5.5.16). Since the manipulations are reversible, as shown in Section 5.2, Eq. (5.5.33) is
397
equivalent to Eq. (5.5.26). The solutions of these equations, with the same initial conditions on v
and v& , must be identical, to within numerical precision used in their solutions. This result
justifies the formal calculation with the trapezoidal integrator in Section 5.5.4 and shows that the
Index 0 DAE solution algorithm has the same convergence properties as the ODE integrator that
is used.
5.5.6 Implicit Index 0 DAE Integration Algorithm
Implicit Index 0 DAE integration in the residual DAE form of Eq. (5.5.16) is as
follows:
velocity constraints. Evaluate the constraint Jacobian F q (q 0 , t 0 ) and matrices U and V in
Eqs. (5.2.5) and (5.2.6). Obtain initial conditions v 0 = 0 and &v 0 = V T&q 0 from Eq. (5.3.2).
Evaluate B 0 of Eq. (5.2.13) and updated in Eq. (5.2.20). Evaluate u = h ( v, t) of Eq. (5.2.14)
and q of Eq. (5.2.33). Evaluate D of Eq. (5.2.24), and q& of Eq. (5.2.34). Evaluate P 2(q, χ ) ,
P3(q, q& ) , P 4(q, η) , M 2(q , μ ) , R &&v , R v& , and R v of Section 5.5.3.
(2) Apply the implicit numerical integrator of Eq. (5.5.23) for the trapezoidal method or of
Eq. (5.5.37) for the Runge-Kutta method to proceed stepwise on a time grid with step size h.
Use Eqs. (5.5.20) or the second and third of Eqs. (5.5.24) to evaluate v n and v& n . In either
case, use Eqs. (5.2.33) trough (5.2.35) to evaluate qn , q& n , and &&qn .
(3) Monitor the condition number of J n , the number of Newton-Raphson iterations required
in Step (2), the norm of v, and the number of iterations required to evaluate u and B. If
tolerances are exceeded, define a new time t 0 and associated q 0 and repeat calculations in
Step (1) to define a new parameterization and initial conditions v 0 and v& 0 . Otherwise,
continue with the same parameterization.
(4) Continue the process until the final time tf is reached, or a singularity due to a faulty
design or model is encountered.
5.5.7 Third Derivatives for Error Control
To implement the integration error control approach presented in Section 4.8.5, in the
present case,
v n = v n -1 + hv& n -1 + (h 2 / 2)&&
v n -1 + (h 3 / 6)&&&
vn -1
the third derivative &&&

vn must be computed. For systems in which the variable t does not appear
explicitly, Eq. (5.5.10) may be differentiated to obtain
{
&&& + ΦqT λ& - MUBg q& + QqA& q
MDv && }
{(
ˆ &&ˆ
+ MDv ) q
+ M Dv( ) + (Φ λˆ ) - ( MUB
&&ˆ
q
T
q
ˆ ˆ gˆ )
q q }
- MU ( Bgˆ )q - MUBg q - QqA q& = 0
where all terms are evaluated at t n -1 . Substituting from Eqs. (5.3.18), (5.3.12), and (5.3.13),
using the fact that operators M2 and P2 are linear in their second arguments and collecting terms,
398
&&& + ΦTq λ& º Rhs
MDv (5.5.38)
where
Rhs = {MUBg q& + Q qA& }q
&& + {MUBP2(q, q &&) - P4( q, λ ) + MUBγ q + QqA }q&
&& ) - M 2(q, q
is evaluated at t n -1 . While there are numerous terms in Eqs. (5.5.38), all must be evaluated in
each iteration of Eq. (5.5.23) for an implicit method, so the additional cost in solving for &&&
vn -1 is
the solution of Eq. (5.5.38). This is not a significant additional cost, as indicated in Section 4.8.5.
For an explicit method such as Nystrom4, the cost of these calculations may be justified if
sufficiently large step sizes can be taken and error control is of value.
In addition to providing information for error control, the third derivative &&&
vn -1 enables
improved accuracy of the estimate for &v&n , namely
v n » &&
&& v n -1 + h&&&
vn -1 (5.5.39)
A redefinition of terms in the tangent space ODE formulation yields an equivalent set of
equations that include Lagrange multipliers of the DAE derived in Section 4.10, which provide
constraint reaction forces. The Index 0 DAE formulation obtained is equivalent to the ODE
formulation and enables direct application of explicit integration formulas and implicit
trapezoidal and Runge-Kutta numerical integration formulas to the underlying ODE, which is
then inflated for solution in the DAE setting. This justifies the name “Index 0 DAE” and
provides a practical foundation for modeling and simulating the effects of friction in Chapter 8,
using constraint reaction forces that are defined by the Lagrange multipliers.
Numerical implementation of explicit and implicit numerical integration formulas proceeds as
in the ODE, with somewhat simplified derivative expressions.
Key Formulas
&& + F Tq λ - MUBg - S - Q A = 0
MDv v(t 0 ) = 0, v& (t 0 ) = V Tq& 0 (5.5.11)
R&&v = MD (
R&v = - MUBg &q + S&q + Q&qA D )
&& - UBg ) - MUBP 2(q, Dv
ìïM 2(q, Dv && - UBg ) - MUBg q + P 4(q, l )üï (5.5.18)
Rv = í ýD
A
( A
)
ïî-Sq - Qq + MUBg &q + S&q + Q&q UB ( P 2(q,&q) + F tq ) ïþ
¶R / ¶l = F qT J n = éë¶R n / ¶&&
v F Tq ùû (5.5.19) (5.5.22)
399
5.6 Numerical Examples with Tangent Space Index 0 DAE
Two versions of a spatial spinning top and a spatial double pendulum are used as
examples to test and evaluate the Index 0 DAE formulation of Section 5.5.
5.6.1 Top with Tip Fixed
5.6.1.1 Spin Stabilized Top
The spin stabilized Top of Section 5.4.2.1 is treated here, using the Index 0 DAE
formulation. Data, constraint equations, kinetic expressions, and derivative operators are as
presented in Section 5.4.2.1. For implicit Index 0 DAE integration, using the residual of Eq.
(5.5.16), derivatives of the residual in Eq. (5.5.18) are
é M 2(q, Dv && - UB g ) - MUBP2(q, Dv
&& - UBg ) - MUB g q + P4(q, λ ) ù
Rv = ê úD
( ) &
êë -S q - Q q + MUBg &q + S&q + Q&q UBP2(q,q)
A A
úû
( )
R &v = - MUBg &q + S&q + Q&qA D
R&&v = MD
The implicit trapezoidal algorithm of Section 5.5.4 is implemented in MATLAB Code
5.6.1a of Appendix 5.A, using data summarized here. Numerical results for this formulation over
a simulation interval of 100 sec, with an integration error tolerance Intol = 10 -4 , an iteration limit
of 6, and a bound of 0.7 on the norm of v are shown in Fig. 5.6.1. As indicated, the transition
from 12 to 12.75 rad/sec initial z ¢ angular velocity results in a tight spiral; i.e., stable vertical
spin of the top. The Nystrom4 method with the explicit integration algorithm of Section 5.5.2 and
the SDIRK54 method with the implicit integration algorithm of Section 5.5.6 yielded essentially
identical results.
(5.2.17) and (5.2.20) is varied to evaluate its influence on constraint error. Maximum norms of
constraint error over the simulation interval presented in Table 5.6.1 show that position, velocity,
and acceleration constraint errors are driven toward zero, to computer precision, as convergence
tolerances in the trapezoidal method are tightened. No significant increase in compute time was
associated with the tighter tolerances. Similar results were obtained with the Nystrom4 and
SDIRK54 methods.
Table 5.6.1 Maximum Constraint Error for Trapezoidal Integrator
Tol. Position Err. Velocity Err. Acceleration Err.
e-6 e-9 e-14 e-12
e-9 e-9 e-14 e-12
e-12 e-12 e-15 e-13
400
Figure 5.6.1 Centroid x-y Trajectories for Spinning Top
In addition to demonstrating that the formulation presented satisfies the equations of
motion and all three forms of kinematic constraint, the total energy predicted in simulation of
this conservative system, using the SDIRK integrator with a step size h = 0.001 sec, had a
maximum variation of 4 .6 6 ´ 1 0 - 5 over the 100 sec simulation shown in the lower left of Fig.
5.6.1. For this Top, gravitational force and constraint reaction forces at the tip create no torque
about the z-axis, so the z component of angular momentum must be conserved (Arnold, 1989).
Maximum deviation of the z component of angular momentum in the simulation was 7 .2 5 ´ 1 0 - 7 .
5.6.1.2 Transient Top
The transient Top of Section 5.4.2.2 is treated here, using the Index 0 DAE formulation.
Data, constraint equations, kinetic expressions, and derivative operators are as presented in
Section 5.4.2.2. Integration Jacobians are as in Section 5.6.1.1.
401
The plot in Fig. 5.6.2 of the y coordinate of the centroid vs time, obtained with
trapezoidal integration in MATLAB Code 5.6.1b of Appendix 5.A, agrees with corresponding
results in (Bruls and Cardona, 2010). Constraint errors were similar to those cited in Table
5.6.1.1. Identical results were obtained with Nystrom4 and SDIRK54 methods.
Figure 5.6.2 y-Coordinate of Centroid for Transient Top

Dynamic response of the transient Top is somewhat more extreme than the spin stabilized
top, and there was modest variation in total energy and y component of angular momentum
relative to the tip, both of which are constant in the exact solution. Simulations were carried out
with step size h = 0.0001. Mean values of y-angular momentum and total energy were - 70.3
and 5,435, respectively. Maximum variations in y-angular momentum (total energy) over the two
-3 -2 -5 -4 -5 -5
second simulation were 3 ´10 (2.7 ´10 ) , 8.3´10 (1.4 ´10 ) , and 8.3´10 (1.9 ´10 ) for
the trapezoidal, Nystrom4, and SDIRK54 integrators, respectively. These results indicate that the
implicit five stage SDIRK54 method preserves invariants more accurately than the explicit four
stage Nystrom4 method or the implicit one stage trapezoidal method. This contrasts with energy
loss predicted by the second order generalized- a integration method used by Bruls and Cardona
(2010), which imposes numerical damping on the solution.
The spatial double pendulum of Section 5.4.4 is treated here, using the Index 0 DAE
formulation. Data, constraint equations, kinetic expressions, and derivative operators are as
presented in Section 5.4.4. Integration Jacobians are as in Section 5.6.1.1.
The plot in Fig 5.6.3 is obtained with the implicit SDIRK54 integrator in MATLAB Code
5.6.2 of Appendix 5.A, using the Index 0 DAE formulation with h = 0.001, a limit of 1 on the
norm of v, and an integration convergence tolerance of 0.01. It shows the x-value of the centroid
vs time for body 2, with Omeg2 = 5 rad/sec and Omeg1 = 0. In explicit Nystrom4 integration,
with a unit norm limit on v and a limit of 500 on condition number of the coefficient matrix, only
one parameterization in 20,000 time steps was required. Likewise, in implicit SDIRK54 and
trapezoidal integration, just one parameterization in 20,000 time steps was required.
402
Figure 5.6.3 x-Coordinate of Centroid of Body 2 vs Time
(5.2.19) and (5.2.22) was varied in Nystrom4, trapezoidal, and SDIRK54 methods to evaluate its
influence on constraint error. Maximum norms of constraint error over the simulation period
presented in Table 5.6.2 show that position, velocity, and acceleration constraint errors are
driven toward zero, to computer precision, as convergence tolerances are tightened. No
significant increase in compute time was associated with the tighter tolerances.
Table 5.6.2 Maximum Constraint Error For Nystrom4 Explicit Integrator
Tol. Position Err. Velocity Err. Acceleration Err.
e-6 e-10 e-7 e-7
e-9 e-12 e-11 e-9
e-12 e-15 e-14 e-13
In addition to demonstrating that the formulation presented satisfies the equations of
motion and all three forms of kinematic constraint, the total energy predicted in simulation of
this conservative system using the SDIRK54 integrator had a maximum variation of 2 .3 ´ 1 0 - 6
over the 20 sec simulation. Further evidence of accuracy of solutions is based on a comparison of
results with the ODE formulation of Section 5.3 that is implemented in Section 5.4.4 with
numerical methods that assure accurate solutions. The maximum deviation over the 20 sec
simulation of solutions obtained using the SDIRK integrator in the present formulation and in the
ODE formulation was 2 .5 ´ 1 0 - 9 for q, 5 .7 ´ 1 0 - 8 for q& , and 7 .3 ´ 1 0 - 7 for q&& . These data
support the validity of the Index 0 DAE formulation applied to the underlying ODE.
Simulations using MATLAB codes of Appendix 5.A confirm that accurate results are obtained
with the Index 0 formulation. Numerical performance in obtaining solutions that satisfy all
three forms of the constraint equations is shown to be comparable to the ODE formulation.
As with implementation of the ODE formulation in Section 5.4, ad-hoc derivation of equations
of motion and programming of solution methods is oppressive. This motivates effort in the
chapter to create computational implementations of both equation formulation and solution.
403
5.7 Code 5.7 for Tangent Space Simulation of Planar Systems
The rudimentary general-purpose MATLAB computer Code 5.7 of Appendix 5.A is
presented to implement the tangent space ODE and Index 0 DAE formulations of Sections 5.3
and 5.5 for planar multibody systems. Revolute, translational, and distance kinematic constraints
of Section 3.2 are implemented, using derivatives presented in Section 3.2 and Appendix 5.B that
are required for explicit Nystrom4 and RKFN45 and implicit Trapezoidal and SDIRK54
numerical integration methods of Section 4.8. Applied, gravitational, and internal forces defined
by translational- and rotational-spring-damper-actuators (TSDA and RSDA) that are presented in
Section 4.5 are implemented, using derivatives of Appendix 5.B. Centroidal body fixed reference
frames for equations of motion derived in Section 4.2 are employed, with derivative expressions
presented in Appendix 5.B. Fixed time step explicit Nystrom4 and variable time step RKFN45
algorithms and implicit variable time step trapezoidal and SDIRK54 algorithms are implemented
for numerical integration of tangent space ODE and Index 0 DAE of planar system dynamics.
in Section 5.8, including those treated with ad-hoc derivations and computer implementations in
Sections 5.4 and 5.6. The essence of computer aided kinematics and dynamics of mechanical
systems is to enable computer formulation and solution of the equations of motion, without the
painful detail of ad-hoc derivation of equations of motion and associated ad-hoc coding of
numerical solution algorithms that are illustrated in Sections 5.4 and 5.6. The computer
implementation presented in this section and applied in Section 5.8 is intended to introduce the
reader to methods that are now available in commercial dynamic simulation software and
advanced software that is likely to appear in the foreseeable future.
5.7.2.
The initial segment of Code 5.7 involves integration and error control parameters that
underlie the tangent space formulation and associated numerical integration methods, as
presented in Fig. 5.7.1. Lines 3 through 17 define parameters that control error in the tangent
space formulations of Sections 5.3 and 5.5 and numerical integration methods of Section 4.8.
Relatively tight error control parameters are used as defaults, which may be modified to seek
greater computer efficiency or to more tightly constrain error. The initial time step is defined in
line 19, followed by the maximum time step that is allowed in variable time step integrators in
line 21. The final simulation time is defined in line 24. The integration option to be used is
selected from among four integrators and two formulations (ODE and Index 0 DAE) shown in
lines 26 through 29, followed by selection of an option in line 31 to invert or factor the
integration Jacobian for implicit integrators in the ODE formulation.
1 %AA Planar Tangent Space Multibody Simulation

2
3 %Integration and Error Control Parameters
4 utol=10^-12; %Tolerance in solving for u
5 Btol=10^-12; %Convergence criteria in B iteration
404
8 Maxv=10; %Limit on norm of v
9 Maxu=10; %Limit on norm of u
10 MaxImpSoliter=10; %Limit on number of implicit integration iterations
11 MaxUiter=8; %Limit on number of U iterations
12 MaxJcond=5000; %Limit on magnitude of Jcond in Trap
13 R1nmax=30000; %Limit on initial residual in implicit integration
14 MaxECond=200; %Limit on magnitude of ECond
15 MaxBCond=600000; %Limit on magnitude of BCond
16 MaxBnormRat=10; %Maximum ratio of Bnorm to Bnorm at parameterization
17 MaxBCondRat=10; %Maximum ratio of BCond to BCond0 at parameterization
18
19 h0=0.001; %Initial time step
20 h=h0;
21 hmax=0.001;
22 hvar=1; %hvar=1, variable h;hvar=2, constant h
23
24 tfinal=2; %Final simulation time
25
26 integ=1; %integ=1-Impl ODE Trap; integ=2-Impl ODE SDIRK54;
27 %integ=3-Impl Ind0 Trap; integ=4-Impl Ind0 SDIRK54;
28 %integ=5-Expl ODE Nystrom4; integ=6-Expl ODE RKFN45;
29 %integ=7-Expl Ind0 Nystrom4; integ=8-Expl Ind0 RKFN45
30
31 InvJ=1; % InvJ=1, Invert J; InvJ=2, Factor J
Figure 5.7.1 Integration and Error Control Parameters
Data for eleven applications that are implemented in Code 5.7 are indexed in lines 35
through 46 of Fig. 5.7.2 to application data files that are defined in the AppData function
presented in Fig 5.7.3. The declaration in line 49 defines which application is implemented in the
simulation to be carried out. The AppData and parameter definitions of lines 51 through 54 are
used throughout the code to pass application data for each simulation. If required for definition
of initial conditions that are consistent with constraints, the user may enter code following line
56. Finally, output data desired are defined for each application, as illustrated for the double
pendulum of app = 1 in lines 382 to 391.
35 %Application Data
36 %app=1, Double Pendulum
38 %app=3, Lumped Mass Coil Spring-5 masses
40 %app=5, Three Body Translational Model
42 %app=7, Rotating Disk with Translating Body
43 %app=8, Multiple Slider-Crank
44 %app=9, Flywheel-Spring
45 %app=10, Loader
46 %app=11, Slider-Crank-2
47
48
49 app=1; %Application to be simulated
405
50
51 [nb,ngc,nh,nc,nv,nu,NTSDA,NRSDA,PJDT,PMDT,PTSDAT,PRSDAT,q0,qd0]=...
52 AppData(app); %AppData(app)
53
54 par=[nb;ngc;nh;nc;nv;nu;g;utol;Btol;intol;Atol;h0;hvar;NTSDA;NRSDA];
55
56 %Initial condition calculation, if required
57 if app==2
58 phi2d0=6;
59 Phiq=PhiqEval(0,q0,PJDT,par);
60 EEE=[Phiq;zeros(1,5),1,zeros(1,6)];
61 RRHS=[zeros(11,1);phi2d0];
62 qd0=EEE\RRHS;
63 end
382 %Data of Interest (Enter for each application)

383 if app==1 %Double Pendulum
384 x1(n)=q(1);
385 y1(n)=q(2);
386 phi1(n)=q(3);
387 x2(n)=q(4);
388 y2(n)=q(5);
389 phi2(n)=q(6);
390 phi2m1(n)=phi2(n)-phi1(n);
391 end
Figure 5.7.2 Application Data in Main Code
The third component of user entered code is the AppData function shown for the double
pendulum in Fig. 5.7.3. The AppData function definition of lines 8 through 51, based on data
entered for each application, is passed to the main code and functions that are executed in calls
throughout the code. Lines 8 through 15 define the dimension of variables that characterize the
model. If entered incorrectly, the code will fail in ways that are difficult to understand from
MATLAB error messages.
The Planar Joint Data Table (PJDT) is defined in lines 21 through 26. A detailed guide
to entering data into the PJDT is provided in Section 3.9.1 and is not repeated here. It contains
data defined in lines 25 and 26, as entered for the planar double pendulum.
The Planar Mass Data Table (PMDT) is defined in lines 28 through 30. It enables entry
of mass and moment of inertia data for each body in the system, as follows:
PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
The Planar TSDA Data Table (PTSDAT) is defined in lines 32 through 39. More
specifically, a template for TSDA T defined in Fig. 4.5.1, repeated here for clarity, is
PTSDAT(:,T)=[i;j;sipr;sjpr;K;C;el0;F];
406
Fig. 4.5.1 Planar TSDA
The user enters i, j, s¢i , s¢j , K, C, el0, and F,
where K is spring constant, C is damping
coefficient, el0 is free length of the spring,
and F is a user defined force, possibly time
dependent.
The Planar RSDA Data Table (PRSDAT) is defined in lines 32 through 39. More
specifically, a template for RSDA R defined in Fig. 4.5.2, repeated here for clarity, is
PRSDAT(:,R)=[i;j;K;C;phi0;T];
Figure 4.5.2 Planar RSDA

The user enters i, j, s¢i , s¢j , K, C, phi0, and T,
coefficient, phi0 is free angle of the spring,
and T is a user defined torque, possibly time
dependent.
Finally, initial generalized coordinates and velocities are defined in lines 48 through 51.
If computation is required to define consistent initial data, code doing so is entered in the main
code, following line 56 in Fig. 5.7.2.

5 %To enable unilateral spring, remove %(comment out) in
6 %QAEval line 41, QAsqqd lines 38 & 55, and Main code line 357
7
11 nc=4; %Number of constraint equations
12 nv=ngc-nc;
13 nu=nc;
14 NTSDA=1; %Number of TSDA force elements
15 NRSDA=2; %Number of RSDA force elements
20
21 %PJDT(12,nh) Joint Data Table
22 %PJDTd(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr];
23 %k=joint No., t=joint type(1=Rev,2=Tran,3=Dist), i&j=bodies conn.,
24 %si&jpr=vectors to Pi&j, d=dist., vi&jpr=vectors along trans axis
25 PJDT(:,1)=[1;1;0;-ux;zer;0;zer;zer]; %Rev-Body 1 to Ground
407
26 PJDT(:,2)=[1;1;2;ux;-ux;0;zer;zer]; %Rev-Body 1 to Body 2
27
28 %PMDT(2,nb) Mass Data Table
29 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
30 PMDT=[[1;0.3],[1;0.3]];
31
32 %PTSDAT(10,NTSDA) TSDA Data Table
33 %PTSDAT(:,T)=[i;j;sipr;sjpr;K;C;el0;F];
34 %T=TSDA No., i&j=bodies conn.,si&jpr=vectors to Pi&j, K=spring constant,
35 %C=damping coefficient,el0=spring free length,F=const. force
36 %Unilateral spring is defined by setting spring constant in QA, QAsq,
37 %QAsdd and TE calculation equal to K*(1-sign(q(1)))/2
38 K=10^5;
39 PTSDAT(:,1)=[1;0;zer;-ux-uy;K;0;1;0];
40
41 %PRSDAT(6,NRSDA): RSDA Data Table
42 %PRSDAT(:,R)=[i;j;K;C;phi0;T];
43 %R=RSDA No., i&j=bodies connected, K=spring constant,
44 %C=damping coefficient,phi0=spring free angle,T=constant torque
45 PRSDAT(:,1)=[1,0,0,0,pi/2,0];
46 PRSDAT(:,2)=[1,2,10^5,10^4,0,0];
47
48 %Initial generalized coordinates
50 q0=[1;0;0;3;0;0];
51 qd0=[0;0;0;0;0;0];
Figure 5.7.3 AppData Function, Double Pendulum
Computational flow in the main program beyond line 100, which requires no input from
the user, is outlined in Fig. 5.7.4. Data for integration that are defined in Section 5.7.1 are
initialized following line 108. Criteria for reparameterization for both implicit and explicit
integrators are defined following lines 142 and 169. Code to implement reparameterization is
called following line 179, if error control tolerances are exceeded. Code that implements the
eight numerical integration methods supported is outlined following line 205. Integration results
for tangent space coordinates (v, vd, and vdd) that are computed using the selected integration
method are processed to obtain physical generalized coordinates (q, qd, qdd, and Lam) following
line 279. Output data of interest for each of the applications discussed in Section 5.7.1 are
defined following line 324. Finally, position, velocity, and acceleration constraint errors are
computed following line 504.
108 %Initialize Data For Integration

142 if integ<5 %Start implicit reparameterization criteria
169 if integ>4 %Start explicit reparameterization criteria

187 if Cr>1 %Reparameterization
192 [vd,vdd,q0,U,V,B,jRepar]=Param(n,tnm,Q,Qd,Qdd,PJDT,par,jRepar);
205 % Integration
206 if integ==1 %Implicit ODE trapezoidal
207 [v,vd,vdd,R1n,ImpSoliter,JCond,Jinv,Jinviter,h,nch]=...
408
208 ImplicitODETrap(n,tn,npar,Vv,Vvd,Vvdd,Uu,q0,V,U,B,...
209 h,hmax,nch,PMDT,PTSDAT,PRSDAT,PJDT,par,InvJ,Jinv);
248 if integ==5 %Explicit ODE Nystrom

249 [v,vd,vdd,ECond]=ExplicitODENystrom4(n,tn,Vv,Vvd,Uu,V,U,B,q0,...
250 h,npar,PMDT,PTSDAT,PRSDAT,PJDT,par);
279 %Process Results

287 %Evaluate q
300 %Evaluate B and qd
314 % Evaluate qdd and Lam
324 %Calculate output data

504 %Calculate constraint error

Computing functions that underlie the main code that is outlined in Fig. 5.7.4 are
identified in Fig. 5.7.5. Computing functions include the Add function that enables adding
nonzero submatrices to sparse matrices, below and to the right of the address of the (1,1) term in
the submatrix, which is added to the underlying matrix that was initialized to zero. The function
ATran evaluates the orientation transformation matrix. Functions BEval and usolve evaluate the
matrix B in Eq. (5.2.20) and dependent generalized coordinates in Eq. (5.2.17) and Param
evaluates terms that are required for the tangent space parameterization of Section 5.2.
Vector partition functions parPart, qPart, and xPart support partitioning of vectors into
components that are used in kinematic and kinetic computations. Similarly, the Constraint and
Data Table Partition Functions provide access to elements of the data tables that are required to
implement computation.
expressions and derivatives defined in Section 3.2 and Appendix 5.B that are needed in
implementing the tangent space ODE and Index 0DAE formulations and numerical integration of
the associated equations of motion. Care is taken to account for the fact that ground is designated
by j = 0 and its generalized coordinates are constant, yielding no derivative contribution, but to
include geometric quantities that define the constraint of body i with ground. Similarly, the
Kinetic and Derivative Evaluation Functions listed evaluate inertial and generalized force terms
and derivatives defined in Sections 4.2, 5.3, 5.5, and Appendix 5.B that are required in
formulation and solution of the equations of motion.
Finally, the numerical integration functions listed carry out the numerical integration
process, where ODEfunct evaluates the tangent space ODE of Section 5.3 and Ind0ODEfunct
evaluates the tangent space Index 0 DAE of Section 5.5, for use in the two explicit numerical
integrators. Similarly, JacobODE and JacobInd0 evaluate the implicit numerical integration
Jacobians defined in Sections 5.3 and 5.5 and ResidODE and ResidInd0 evaluate the associated
residuals of the equations of motion for use in the implicit numerical integrators.
Internal details of the Computing Functions listed in Fig. 5.7.5 are not presented here,
since each of the functions is documented internally and the user need not modify these functions
in applications. In comparison to computing functions for planar kinematic analysis in Fig. 3.9.5,
409
however, it is noted that many additional functions are required. Basic kinematic computing
functions are identical in both codes, but additional kinematic computing functions appear that
support implicit numerical integration; e.g., GamsqqdEval, P3Eval, and P4Eval. New computing
functions, of course, appear for dynamics.
Computing Functions
Add
ATran
BEval
usolv
Param
User Input Function
AppData
parPart
qPart
xPart
Constraint and Data Table Partition Functions
DistPart
RevPart
TranPart
PTSDATPart
PRSDATPart
PhiEval
PhiqEval
GamEval
GamsqqdEval
P2Eval
P3Eval
P4Eval
P5Eval
Kinetic and Derivative Evaluation Functions
MEval
M2Eval
QAEval
QAsqqd
Numerical Integration Functions
ExplicitNystrom4
ExplicitRKFN45
ODEfunct
Ind0ODEfunct
ImplicitTrap
ImplicitSDIRK54
JacobODE
JacobInd0
ResidODE
ResidInd0
410
5.7.3 Code Output
reports the following arrays of values at each time step that shed light on the application and
performance of the code:
PosConstrNorm; norm of position constraint error
VelConstrNorm; norm of position constraint error
AccConstrNorm; norm of acceleration constraint error
Biterrpt; number of iterations required to update B
Iterurpt; number of iterations required to update u
hrpt; stepsize h for variable step integrators
ImpSoliterrpt; number of iterations required in implicit integration
JCondrpt; Condition number of matrix in implicit equation solution
jRepar; total number of reparameterizations in simulation
Q; array of values of q
Qd; array of values of q&
Qdd; array of values of q &&
Vv; array of values of v
Vvd; array of values of v&
Vvdd; array of values of &v&
LLam; array of values of Lagrange multiplier vector l
TE; total energy of system
Code 5.7 of Appendix 5.A is a tool for numerical simulation and experimentation with the
tangent space ODE and Index 0 DAE formulations of planar multibody dynamics. It is typical
of code that enables the engineer to create models of mechanical systems, carry out
simulations, and investigate the effects of model and computational variations, without
investing extensive time and effort in ad-hoc derivation and coding of specific examples.
Development and use of such codes is the essence of computer aided kinematics and dynamics
of mechanical systems.
411
5.8 Planar System Simulation Using Code 5.7
Six planar systems are simulated using the general-purpose tangent space MATLAB
Code 5.7 of Appendix 5.A. Kinematic and kinetic characteristics of the models are defined and
entered in the AppData function defined in Section 5.7. Code 5.7 is then run and results are
analyzed. The technical complexity of ad-hoc derivation of equations and ad-hoc coding that are
characteristic of examples presented earlier in the chapter is contrasted with the ease of use of a
general-purpose computer code. This is the essence of the field of computational dynamics.
5.8.1 Double Pendulum
5.8.1.1 Model and Data Set
The planar double pendulum that is treated with ad-hoc derivation and numerical
implementation in Section 5.4.1 is shown in Fig. 5.8.1, with a unilateral spring K3 between
body 1 and ground. The AppData function for this model, using data of Section 5.4.1, is shown
in Fig. 5.8.2 for the bodies in an initial configuration along the global x axis and a free angle of
RSDA1 of zero radians.
Figure 5.8.1 Double Pendulum

5
10 nv=ngc-nc;
11 nu=nc;
14
15 ux=[1;0];
16 uy=[0;1];
17 zer=zeros(2,1);
18
20 %PJTd(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr]; k=joint No.,
21 %t=joint type(1=Rev,2=Tran,3=Dist), i&j=bodies conn.,
412
25
27 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
28 PMDT=[[1;0.3],[1;0.3]];
29
31 %PTSDAT(:,T)=[i;j;sipr;sjpr;K;C;el0;F]; T=TSDA No.,
32 %i&j=bodies conn.,si&jpr=vectors to Pi&j, K=spring constant,
34 %Unilateral spring is defined by setting spring constant in QA, QAsq,
35 %QAsdd and TE calculation equal to K*(1-sign(q(1)))/2
36 K=10^4;
37 PTSDAT(:,1)=[1;0;zer;-ux-uy;K;0;1;0];
38
40 %PRSDAT(:,R)=[i;j;K;C;phi0;T]; R=TSDA No.,
41 %i&j=bodies connected, K=spring constant,
43 PRSDAT(:,1)=[1,0,0,0,0,0];
44 PRSDAT(:,2)=[1,2,10^2,10^2,0,0];
45
47
48 q0=[1;0;0;3;0;0];
49 qd0=[0;0;0;0;0;0];
50
51 end
Figure 5.8.2 AppData Function, Double Pendulum
5.8.1.2 Nominal Results
Numerical results for 10 sec simulations, carried out with K3 = 0 , other data as in
Section 5.4.1, and all four integrators, are identical to those reported in Section 5.4.1. MATLAB
timing results for the simulation shown on the right of Fig. 5.4.2 indicate that CPU time using the
general-purpose Code 5.7 on a laptop workstation was approximately three times that required
by the hand derived and programmed Code 5.4.1 in Appendix 5.A, for the same simulation data
and integrator. Specifically, for the 10 sec simulation reported on the right of Fig. 5.4.2 using the
trapezoidal integrator, Code 5.7 required 34 CPU sec and Code 5.4.1 required 11 CPU sec. The
23 CPU sec. additional cost with the general purpose code more than pays for the enormous
difference in person hours required in the ad-hoc implementation. See Section 5.8.1.5 for details.
5.8.1.3 Results for Stiff Systems
As a test of the ability of explicit and implicit integration methods to accurately simulate
moderately stiff systems, spring and damping parameters were set to K1 = 20, C1 = 10, K2 = C2 =
1,000, and K 3 = 0 . Initial conditions are as in Section 5.4.1. This models a relatively stiff single
bar flexible pendulum. Results presented in Fig. 5.8.3 are obtained using the SDIRK54 implicit
integration method with variable step size and an error tolerance Atol = e-5. This integrator is the
best of the six used in handling stiff systems and yielded results that approximate swinging motion
of a single flexible bar, including damping relative motion of the two bodies that make up the
413
model and settling to a vertical downward configuration. A simulation carried out with the explicit
Nystrom4 integration method with a constant step size h = e-3 yields erroneous results. This is to
be expected when using an explicit integrator to simulate a stiff system. With this moderately stiff
example, setting h = e-5 yielded results with the Nystrom4 method that are close to those in Fig.
5.8.3. Simulations carried out for this moderately stiff example with h = e-3 in the implicit
trapezoidal integrator yielded reasonable results, even though it is known to be less stable than
SDIRK54. Finally, the explicit variable step size RKFN45 method yielded accurate results, at the
cost of reducing step sizes well below e-3.
Figure 5.8.3 Simulations with Moderately Stiff System; K1=20, C1=10, K2=C2=1,000
A more severe test of the effect of stiffness on numerical performance of solution
methods is obtained by setting K1 = 20, C1 = 10, K2 = C2 = e6, and K 3 = 0 . Initial conditions
are as above. In this case, all explicit integrators failed to even complete a simulation. The
SDIRK54 integrator with maximum step size e-3 yielded the solution of Fig. 5.8.4. The implicit
trapezoidal method with constant step sizes e-3 through e-5 provided qualitatively reasonable
solutions over the simulation interval, but failed to accurately simulate the initial transient. The
reader is encouraged to use MATLAB Codes 5.4.1 and 5.7 in Appendix 5.A to carry out
simulations with varying parameter values to gain an appreciation for the strengths and
weaknesses of the various numerical integration methods. Constraint errors encountered in all
simulations reported in this section were comparable to values reported in Table 5.4.1 of Section
5.4.1.
Figure 5.8.4 Simulations with Stiff System; K1 = 20, C1 = 10, K2 = C2 = 106 , K3 = 0

Finally, a spectrum of simulations was carried out using Code 5.7 with K1 = C1 = 0 ,
initial conditions with the bars initially at rest along the x axis; i.e., q& 0 = 0 and
q 0 = [1 0 0 ] , constant or maximum step size h = 0.001, intol=e-6, utol=Btol=e-12,
T
0 3 0
and Atol=e-5 for a range of values of K 2 , C2 , and K 3 . The data set for these simulations is given
414
in Fig. 5.8.2. In ten second simulations, the bars fall due to gravity until the first bar is vertically
downward, where it encounters the unilateral spring shown in Fig. 5.8.1. The first bar deforms
the unilateral spring, the torsional spring and damper between the bars are activated, and the bars
rebound toward their initial configuration.
Table 5.8.1 is a tabulation of successful and failed simulations carried out with three
variable step integration methods, for spring and damper values given in the bottom three rows
of the table. A number in the first three rows of the table is the average step size for an ODE
simulation that ran successfully and an x denotes a simulation that failed. The fixed step size
explicit Nystrom4 method failed for even modest stiffness. The variable step size explicit
RKFN45 method adapts to treat some stiffness, but fails long before the implicit methods. As
noted in the second column, it succeeds only by tracking the high frequency response with a
small step size, even though the amplitude of oscillation is very small. The implicit trapezoidal
method successfully simulates quite stiff systems, as well as the SDIRK54 implicit method that is
among the best integrators for stiff systems. Data in Table 8.5.1 are for the ODE formulation.
Slightly smaller step sizes were required for the Index0 formulation, which performed
comparably.
Table 5.8.1 Simulation Success (Avg. Step Size h) and Failure (x)
RKFN45 8.5e-4 8.6e-5 x x x x
trapezoidal 6.9e-4 7.2e-4 7.3e-4 4.5e-4 1.5e-4 x
SDIRK54 1e-3 9.6e-4 9.6e-4 9.8e-4 1e-3 x
K2 e4 e5 e6 e7 e8 e9
C2 e3 e4 e5 e6 e7 e8
K3 e4 e5 e6 e7 e8 e9
To see the effect of stiffness and the transient response due to the stiff unilateral spring
K3 , stiff spring K 2 , and high damping C2 , 10 sec simulation results for data in the third column
of Table 5.8.1 ( K 2 = K3 = 105 and C 2 = 104 ) with the SDIRK54 algorithm are presented in Fig.
5.8.5. Even with this level of stiffness, only a single parameterization was required in 32,049
time steps and maximum norms of position, velocity, and acceleration constraint errors were
1.2e-15, 1.2. The top plot in Fig. 5.8.5 shows the angle phi2. As the first bar encounters the
unilateral spring, the pendulum rebounds, but loses amplitude due to damping and associated
reduction in total energy. The second plot shows that the relative angle phi2-phi1 between the
bars is activated for short periods of time, due to impact, and is rapidly damped out. The total
energy shown in the third plot confirms that energy is lost due to high damping over short
periods of time following impacts. The bottom plot shows that step size h is reduced significantly
in neighborhoods of impacts and increases to the maximum h = e-3 that is allowed after transient
oscillations dissipate. This illustrates how an implicit integrator with variable step size adapts to
stiff transients, whereas explicit integrators cannot.
415
Figure 5.8.5 Simulation with K1=C1=0, K2=K3=105 , C2= 10 4 , Atol= 10-5
5.8.1.4 Results for Highly Oscillatory System
To see the influence of high damping, coupled with stiff springs, in rapidly reducing
transient response and enabling implicit integrators to reduce step size during transients and
increase step size as transients damp out, it is instructive to retain high spring stiffness and
reduce damping to zero. Setting K1 = C1 = C2 = 0, K2 = K3 = 105 , Atol= 10-5 , and the
maximum allowed step size h max = 10 -3 , simulations are carried out with the variable step size
416
explicit RKFN45 implicit algorithm. In contrast with the ability of the implicit algorithm shown
in Fig. 5.8.5 to reduce step size during transients and to increase step size after damping of the
transient, as shown in Fig. 5.8.6, in this example the algorithm is forced to track high frequency
response throughout the simulation. This is because the transient does not damp out. Thus, this
example is simply highly oscillatory and not stiff. It is interesting to note that the explicit
RKFN45 algorithm performs as well in this situation as the implicit SDIRK54 algorithm.
Figure 5.8.6 Simulation with RKFN45 and K1=C1=C2=0, K2=K3= 105 , Atol= 10-5
5.8.1.5 Level of Effort Required
From the point of view of personnel time involved in the two approaches, compare the
few minutes required to create the data set in Fig. 5.8.2 and launching the general-purpose code
with the time required to derive the extensive expressions in Section 5.4.1 and to program and
debug the ad-hoc Code 5.4.1. Even with the greatest of care, ad-hoc derivation of constrained
equations of motion and the numerous derivatives that are required for numerical solution is time
consuming and error prone. Finding analytical derivation errors is time consuming and painful.
Even more time consuming and painful is the process of creating and debugging ad-hoc
417
computer code to implement numerical solution methods. A minimum of several person-days of
effort is required by even the most proficient analyst/programmer for such an application, much
less the extensive time required by we mere mortals. In contrast, implementing the same
simulations with data presented in Fig. 5.8.2 and launching simulations with the well debugged
Code 5.7 in Appendix 5.A required only a few person-minutes of effort. Considering the high
cost of engineering labor and the minimal cost of digital computer CPU time, the 23 CPU sec
penalty in CPU time reported above is a bargain.
The quick return mechanism shown in Fig. 5.8.6, whose kinematics were studied in
Section 3.10.2, is driven by counterclockwise rotation of the flywheel (body 2) to cause
oscillation of body 1 via the body 3 key that translates in the slot shown in body 1. The
connecting rod between point P1 on body 1 and the centroid of the cutting tool (body 4) causes
body 4 to move slowly to the left in a cutting stroke and to return more quickly to the right for
the next cutting stroke.
Figure 5.8.6 Quick Return Mechanism

The radius of the flywheel is 2 m, the distance between points O and P1 on body 1 is 4 m,
and the length of the connecting rod that is modeled as a distance constraint is 2.5298 m. The
centroid of body one is at its midpoint. The initial configuration of body 1 is r 0 = [1.2 1.6] and
T
f10 = 0.9273 rad. With m1 = J1 = 100 , m2 = J 2 = 1000 , m3 = J3 = 1, and m4 = J 4 = 50 in SI units,

the data set for the system in the AppData function is given in Fig. 5.8.7. Initial velocities, with
an initial angular velocity f& 02 = 6 rad/sec of body 2, are calculated in lines 50 through 57 of the
main code of Fig. 5.7.2.

55
418
60 nv=ngc-nc; %Number of independent coordinates
61 nu=nc; %Number of dependent coordinates
64
65 ux=[1;0];
66 uy=[0;1];
67 zer=zeros(2,1);
68
69 %PJDT(12,nh): Joint Data Table
79
80 %PMDT(2,nb): Mass Data Table
81 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
82 PMDT=[[100;100],[1000;1000],[1;1],[50;50]];
83
84 %PTSDAT(10,NTSDA): TSDA Data Table
86 %i&j=bodies connected, si&jpr=vectors to Pi&j, K=spring constant,
87 %C=damping coefficient,el0=spring free length,F=constant force
88 PTSDAT=zeros(10,1);
89
94 PRSDAT=zeros(6,1);
95
97 q10=[1.2;1.6;0.9273];
98 q20=[0;2;0];
99 q30=[1.5;2;0.9273];
100 q40=[0;4;0];
101
102 q0=[q10;q20;q30;q40];
103 qd0=zeros(12,1); %Placeholder, qd0 calculated in main program
Figure 5.8.7 AppData Function, Quick Return Mechanism
5.8.2.2 Simulation Results and Analysis
Results of a simulation carried out with tolerances of Fig. 5.7.1, the variable step size
trapezoidal ODE integrator with tolerances utol = Btol = e-8, Atol = e-5, hmax = e-3, and a final
time tf = 2.5 sec are presented in Fig. 5.8.8. The cutting stroke that takes place over the first 0.6
419
sec of the stroke is at a speed less than 10 m/sec, whereas the top speed during the return stroke
is 25 m/sec, thus the name “quick return”. The rapid change in velocity results in high
accelerations. No cutting forces are included in this simulation, so the system should be
conservative. To confirm this is true, total energy is constant to four decimal places.
Figure 5.8.8 Quick Return Cutter Position, Velocity, and Acceleration vs Time
Eleven reparameterizations were required in 4,916 time steps (447 time steps per
reparameterization), based on the criteria that the condition number of matrix B increases no
more than a factor of 10, relative to its value in the last parameterization where it was defined.
Maximum norms of position, velocity, and acceleration constraint errors were 4e-11, 4e-9, and
2e-7, respectively. Comparable results were obtained using the implicit SDIRK54 ODE
integrator and the explicit Nystrom and RKFN45 ODE integrators. Success of the explicit
integrators indicates that this system is not stiff. Simulations carried out using all four integrators
with the Index0 formulation yielded identical results and comparable numerical performance.
To see differences in solution obtained with the trapezoidal and more accurate SDIRK54
integrators, norms of the difference between system position (q diff), velocity (qd diff), and
acceleration (qdd diff) predicted by the two integrators in the ODE formulation are shown in Fig.
420
5.8.9. From the x4 position and velocity plots in Fig. 5.8.8, it is clear that relatively smooth
behavior occurs before approximately t = 1 sec, hence the small norms of position and velocity
difference before this time, after which slight phase differences lead to larger norms of
differences.
Figure 5.8.9 Quick Return, Norm Trap-SDIRK54 Solution Differences vs Time

The acceleration plot in Fig. 5.8.8 suggests that severe accelerations may lead to
differences in prediction accuracies. This is born out in the top plot in Fig. 5.8.9. Recall that the
norms presented in Fig. 5.8.9 are of the full vectors of 12 generalized coordinates. Individual
generalized coordinate plots differ only slightly between the two approximate solutions, phase
differences leading to the magnitudes of norms of differences.
5.8.2.3 Level of Effort Required
MATLAB timing routines reported 59 CPU seconds for each of the foregoing
simulations, after less than two hours of person time to create the data set of Fig. 5.8.4 and run a
simulation. Extending the argument of Section 5.8.1, numerous person days would be required in
ad-hoc derivation and debugging of equations of motion and derivatives required for numerical
integration. Even more time and pain would be required in creating and debugging ad-hoc
computer code that might reduce the CPU time for simulation to approximately 33 cpu seconds.
421
At essentially 0$ per CPU minute on a laptop workstation, trading the numerous person days of
effort and the uncertainty in accuracy of results for half a CPU minute increase in computer time
is clearly the preferred option.
5.8.3 Surge Waves in a Coil Spring
In many applications, a relatively long coil spring is used to arrest the motion of a
moving body and to return it to its original position. While a coil spring of length 1 m is a
continuum of mass M kg and stiffness K n/m for the full spring, a reasonable model is formed by
discretizing the mass and stiffness of the spring. The lumped mass model shown in Fig. 5.8.10 is
constructed by dividing the spring into 5 equal lengths 0.2 m, each with stiffness k = 5K, and
approximating the total mass as 5 lumped masses, m = M/5 each. For computation, M = 1 kg, K
= 1000 N/m, and the spring has length L = 1 m. The initial position of mass i is xi = 0.2i m, i =
1, ..., 5. To study surge wave behavior, motion of the system is initiated by giving mass 5 an
initial velocity 1 m/sec in the negative x direction, which is induced by impact with another
body. The initial velocity of the first four masses is zero.
Figure 5.8.10 Lumped Mass Model of Coil Spring

Kinematics of the system is defined by prescribing a translational joint between each of
the masses and ground. Each spring is attached at the centers of mass of the bodies it connects.
The mass of each body is m = 0.2 kg, and each spring has spring rate k = 5000 N/m and free
length l 0 = 0.2 m. Data for the simulation are given in the AppData function in Fig. 5.8.11.
107 if app==3 %Lumped Mass Coil Spring-5 masses

108
113 nv=ngc-nc;
114 nu=nc;
117
118 ux=[1;0];
119 uy=[0;1];
120 zer=zeros(2,1);
121
123 %PJDT(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr]; k=joint No.,
124 %t=joint type(1=Rev,2=Tran,3=Dist,4=RevClr), i&j=bodies conn.,
126 PJDT(:,1)=[2;1;0;zer;zer;0;ux;ux]; %Tran.-Body 1 to Ground
422
131
132 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]] Mass Data TAble
133 PMDT(:,1)=[0.2;0.1];
134 PMDT(:,2)=[0.2;0.1];
135 PMDT(:,3)=[0.2;0.1];
136 PMDT(:,4)=[0.2;0.1];
137 PMDT(:,5)=[0.2;0.1];
138
139 %PTSDAT(10,NTSDA) Planar TSDA Data Table
141 %i&j=bodies conn.,sipr&sjpr=vectors to Pi&Pj, K=spring constant,
143 PTSDAT(:,1)=[1;0;zer;zer;5000;0;0.2;0]; %Body 1 to Ground
144 PTSDAT(:,2)=[1;2;zer;zer;5000;0;0.2;0]; %Body 1 to 2
148
154
156
157 q0=[0.2;0;0;0.4;0;0;0.6;0;0;0.8;0;0;1;0;0];
158 qd0=[zeros(12,1);-1;0;0];
159
160 end
Figure 5.8.11 AppData Function, Five Lumped Mass Coil Spring Model
Numerical results of this dynamic simulation, carried out using trapezoidal integration in
the ODE formulation with Code 5.7 of Appendix 5.A, include plots of displacements delxi =
x i - x i0 , i = 1,...,5, versus time for each lumped mass in Fig. 5.8.12. Note that with even this
crude model, the wave nature of motion is clear. Mass 1 remains stationary for 0.017 sec, until it
is encountered by the wave. It experiences reflection at the left end and begins rebound at 0.035
sec. The resulting wave to the right reaches mass 5 at 0.052 sec, the same delay as the initial
wave reaching mass 1. This yields an estimate of the wave speed as 1 m/0.017 sec = 59 m/sec .
The theoretical wave speed is (Goldstein, 1980) K/M = 1000 = 31.6 m/sec . Due to the lack
of sharpness in wave behavior of the lumped mass model, the wave speed predicted with the
multibody simulation is only a rough approximation.
423
Figure 5.8.12 Lumped Mass Displacements Versus Time -5 Mass Model
To obtain a better estimate of wave speed, a 10 lumped mass model is defined in App 4
of MATLAB code 5.7. In this model, each lumped mass is 0.1 kg and the springs have stiffness
10,000 N/m. Plots of displacement of masses 1 and 10 are presented in Fig. 5.8.13. In this model,
mass 1 remains stationary for 0.024 sec, until it is encountered by the wave. It experiences
reflection at the left end and begins rebound at 0.037 sec. The resulting wave to the right reaches
mass 10 at 0.061 sec, the same delay as the initial wave reaching mass 1. This yields an estimate
of the wave speed as 1 m/0.024 sec = 42 m/sec , a better estimate of wave speed, but still high.
424
Simulations carried out with all seven of the other formulations and integrators in Code
5.7 yielded essentially identical results.
5.8.4 Three-Body Mechanism with Translational Joints in Ground
The three-body mechanism shown in Fig. 5.8.14 is comprised of translational joints
between each of the bodies and one of the axes in ground, a distance constraint between bodies
one and two, and springs between body one and ground and between bodies two and three. The
body fixed x axis in each body is along the long dimension of the body. Vectors that define the
joint between body one and ground are v1¢ = u x and v¢0 = u y and vectors defining the joints
between bodies two and three and ground are v¢2 = v¢3 = u x and v¢0 = ux . The length of the
distance constraint is l = 5 m , spring constants are k1 = k2 = 10 N/m , masses and moments of
inertia of the bodies are m1 = 5 kg, m 2 = m 3 = 2 kg, and J1 = J 2 = J 3 = 1 kgm 2 , and initial
conditions are q1 = [ 0 0 p / 2] , q 2 = [5 0 0] , q 2 = [ 6 0 0 ] , q& 1 = q& 2 = 0 , and
T T T
q& 3 [ -1 0 0] . The data set for simulation of this mechanism using Code 5.7 is presented in the
T
AppData function of Fig. 5.8.15.
Figure 5.8.14 Mechanism with Translational Joints in Ground
425
236 if app==5 %Three Body Translational Model
241 nv=ngc-nc;
242 nu=nc;
245
246 ux=[1;0];
247 uy=[0;1];
248 zer=zeros(2,1);
249
251 %PJDT(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr;R;mus;mud];
253 %sipr&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=vectors along trans axis,
254 PJDT(:,1)=[2;1;0;zer;zer;0;0.1*ux;uy]; %Tran-Bod1 to ground
255 PJDT(:,2)=[2;2;0;zer;zer;0;0.1*ux;ux]; %Tran-Bod2 to ground
256 PJDT(:,3)=[2;3;0;zer;zer;0;0.1*ux;ux]; %Tran-Bod3 to ground
257 PJDT(:,4)=[3;1;2;zer;zer;5;zer;zer]; %Dist.-Bod1 to Bod2
258
260 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
261 PMDT=[[5;1],[2;1],[2;1]];
262
267 PTSDAT(:,1)=[1;0;zer;-10*uy;10;0;10;0]; %Bod1 to Grnd
268 PTSDAT(:,2)=[2;3;zer;10*ux;10;0;11;0]; %Bod2 to bod3
269
275
277
278 q0=[0;0;pi/2;5*ux;0;6*ux;0];
279 qd0=[0;0;0;0;0;0;-1;0;0];
280
281 end
Figure 5.8.15 AppData Function for Three Body Translational Model
426
Plots of position and velocity of the three bodies predicted by simulation using the
trapezoidal integrator with the ODE formulation in Code 5.7 are presented in Fig. 5.8.16. Eight
reparameterizations were required in 4,000 time steps (500 time steps per reparameterization)
and maximum norms of position, velocity, and acceleration constraint error were 5e-15, 5e-14,
and 2.5e-13, respectively. All eight formulation and integrator combinations supported by Code
5.7 yielded essentially identical results.
Figure 5.8.16 Position and Velocity of Three Body Translational Model

5.8.5 Slider-Crank
Dynamics of the two-body model of a slider-crank mechanism that was analyzed
kinematically in Section 3.2.3.3, shown in Fig. 5.8.17, is analyzed here. The radius of the crank
is 1m and masses and moments of inertia of bodies one and two are m1 = 5 kg , J1 = 5 kgm 2 ,
m 2 = 1 kg , and J 2 = 1 kgm 2 . Initial conditions with l = 1.5 m are q1 = [ 0 0 0] ,
T
q 2 = [ 2.5 0 0] , q& 1 = [ 0 0 100] , and q& 2 = [ 0 0 0 ] . The data set for simulation of this
T T T
mechanism using Code 5.7 is presented in the AppData function of Fig. 5.8.18.
Figure 5.8.17 Two Body Model of Slider-Crank Mechanism

285
427
290 nv=ngc-nc;
291 nu=nc;
294
295 ux=[1;0];
296 uy=[0;1];
297 zer=zeros(2,1);
298
302 %sipr&sjpr=vectors to Pi&Pj,d=dist.,vipr&vjpr=trans axis vectors
306
308 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
309 PMDT=[[5;5],[1;1]];
310
316
322
324
325 q0=[0;0;0;2.01*ux;0];
326 qd0=[0;0;100;0;0;0];
327
328 end
Figure 5.8.18 AppData Function, Two Body Slider-Crank
Plots of angular velocity, angular acceleration, and revolute joint bearing load, computed
from Lagrange multipliers and divided by 10 are shown in Fig. 5.8.19, for connecting rod lengths
varying from 1.5 m down to 1.01 m. Since connecting rod length of 1m is a singular
configuration, large accelerations and bearing loads are expected as connecting rod length
approaches 1 m. All implicit integration formulations performed well, even in the near singular
configuration, where explicit integration formulations failed. Even in the near singular
configuration, implicit integrators led to displacement, velocity, and acceleration constraint error
norms less than 2.5e-14, 4.5e-12, and e-9, respectively.
428
Figure 5.8.19 Angular Velocity and Acceleration and Revolute Joint Bearing Load
5.8.6 Rotating Disk with Translating Body
The disk (body 1) in Fig. 5.8.20 rotates in ground (body 0) about the origin of the x-y
inertial frame. Body 2 translates in body 1 along a guide in the disk that is parallel to the body
fixed y1 axis, one unit to the right of the y1 axis. A TSDA with spring constant K = 10 N/m,
damping constant C = 0, and free length of 10.1 m acts between the origin of the x 2 -y2 reference
frame and a point u1x¢ + 10u1y¢ in body 1. Initial conditions for the system are q10 = [0 0 0] ,
T
q& 10 = [0 0 0] , q 02 = [1 0 0] , and q& 02 = [ 0 0 0 ] . The masses and moments of inertia of

T T T
the bodies are m1 = 10 kg, J1 = 10 kg × m 2 , m 2 = 5 kg, and J1 = 5 kg × m 2 . To simulate motion in a

horizontal plane the acceleration due to gravity is set to zero; i.e., g = 0. The AppData function
for simulation of this system using Code 5.7 is shown in Fig. 5.8.21.
429
Figure 5.8.20 Rotating Disk with Translating Body

285
290 nv=ngc-nc;
291 nu=nc;
294
295 ux=[1;0];
296 uy=[0;1];
297 zer=zeros(2,1);
298
302 %sipr&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=trans axis vectors
306
308 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
309 PMDT=[[5;5],[1;1]];
310
316
430
322
324
325 q0=[0;0;0;2.01*ux;0];
326 qd0=[0;0;100;0;0;0];
Figure 5.8.21 AppData Function for Rotating Disk with Translating Body
Simulation is carried out over a 10 sec interval, with plots of rotation of body 1, f1 =
phi1. and translation of body 2 relative to body 1, y¢2 = dely2pr, shown in Fig. 5.8.22. As shown,
since the spring is precompressed, the disk initially rotates counterclockwise and body 2 moves
in the negative y¢2 direction. With the same data, but damping constant C = 1 Nsec/m, the
dynamic response is shown in Fig. 5.8.23. Identical results were obtained with all eight
integration formulations, for both data sets.
Figure 5.8.22 f1 = phi1 and dy¢2 = dely2pr, No Damping
Figure 5.8.23 f1 = phi1 and dy¢2 = dely2pr, Damping C = 1 Nsec/m
431
Modeling and simulation of planar multibody systems using Code 5.7 of Appendix 5.A
illustrates properties of the tangent space formulation and numerical methods in accurately
forming and solving the ODE and Index 0 DAE of planar system dynamics. Through study of
the behavior of concrete examples, the engineer can gain a practical appreciation of the
theoretical underpinnings of the methods. This is especially important for systems that exhibit
stiff behavior.
Examples presented make clear the disparity in engineering time and effort between ad-hoc
model definition, equation derivation, and coding and debugging and use of general-purpose
software as a means to achieve effective simulation and dynamic system analysis.
432
5.9 Code 5.9 for Tangent Space Simulation of Spatial Systems
The rudimentary general-purpose Code 5.9 of Appendix 5.A implements the tangent
space ODE and Index 0 DAE formulations of Sections 5.3 and 5.5 for spatial multibody systems.
Building block distance, spherical, dot1, and dot2 constraints are implemented and serve as the
foundation for cylindrical, revolute, translational, distance, universal, strut, and revolute-
spherical constraints of Section 3.3. Derivatives presented in Section 3.3 and Appendix 5.B are
used to implement explicit and implicit numerical integration methods of Section 4.8 for solution
of tangent space ODE and Index 0 DAE. Applied, gravitational, and internal forces defined by
translational-spring-damper-actuators (TSDA) presented in Section 4.5 are implemented.
Centroidal body fixed reference frames for equations of motion derived in Section 4.3 are
employed. The same fixed and variable time step explicit and implicit integration algorithms
used in planar system Code 5.7 are implemented for numerical integration of tangent space ODE
and Index 0 DAE of spatial system dynamics.
in Section 5.10, including those treated with ad-hoc derivations and computer implementations in
Sections 5.4 and 5.6. The essence of computational methods in mechanical system dynamics is
enabling computer formulation and solution of the equations of motion, without the painful detail
of ad-hoc derivation of equations of motion and ad-hoc coding of numerical solution algorithms
experienced in Sections 5.4 and 5.6. The computer implementation presented in this section is
intended to introduce the reader to methods that are now available in commercial dynamic
simulation software and advanced software that is likely to appear in the foreseeable future.
5.9.2. The structure of Code 5.9 is identical to that of Code 5.7 presented in Section 5.7.
References are made to that code, where appropriate, to avoid unnecessary repetition.
The initial segment of Code 5.9 defines integration and error control parameters that
underlie the tangent space formulation and associated numerical integration methods. Since it is
identical to the code of Fig 5.7.1, the reader is referred to Section 5.7.1 for discussion of its use.
Application data are indexed in lines 33 to 39 of Fig. 5.9.1 to 6 specific applications that
are defined in the AppData function presented in Fig. 5.9.2. The declaration in line 41 defines
which application is implemented in the simulation. The AppData function parameter definitions
of lines 43 through 45 are used throughout the code to pass data for each simulation. If required
for definition of initial conditions that are consistent with constraint, the user may enter code
following line 47. Finally, desired output data are defined for each application in lines 354 to
416; e.g., lines 356 to 360 for app 1, the spin stabilized top.
34 %app=1, Spin Stabilized Top
35 %app=2, Spatial Double Pendulum
36 %app=3, One Body Cylindrical with Spring
37 %app=4, Spatial Slider-Crank
38 %app=5, 4-Translating Mass Model
39 %app=6, Rotating Disk with Translating Body
433
40
41 app=1; %Application to be simulated
42
43 [nb,ngc,nh,nc,nv,nu,NTSDA,SJDT,SMDT,STSDAT,q0,qd0]=AppData(app);
44
45 par=[nb;ngc;nh;nc;nv;nu;g;utol;Btol;intol;Atol;h0;hvar;NTSDA];
46

355
356 if app==1 %Spin Stabilized Top
357 x1(n)=q(1);
358 y1(n)=q(2);
359 z1(n)=q(3);
360 end
The third component of user entered code is the AppData function shown for the spin
stabilized top in Fig. 5.9.2. The AppData function definition, based on data entered, is passed to
the main code for functions that are executed in calls throughout the code. Lines 14 through 23
define the dimension of variables in the model. If entered incorrectly, the code will fail in ways
that are difficult to understand from MATLAB error messages. The Spatial Joint Data Table
(SJDT) is defined in lines 30 through 34. It contains data defined in Line 36, as entered for the
spin stabilized Top. A detailed guide to entering data into the SJDT is provided in Section 3.11.1
and is not repeated here.
The Spatial Mass Data Table (SMDT) is defined in lines 38 through 40. It contains mass
and moment of inertia data for each body in the system, as follows:
SMDT=[[m1;J11;J12,J13],...,[mnb;Jnb1;Jnb2;Jnb3]]
The Spatial TSDA Data Table (STSDAT) is defined in lines 42 through 48. More
specifically, a template for TSDAT defined in Fig. 4.5.1, repeated here for clarity, is
STSDAT(:,T)=[i;j;sipr;sjpr;K;C;el0;F];
Figure 4.5.1 Spatial TSDA

The user enters i, j, s¢i , s¢j , K, C, el0, and F,
coefficient, el0 is free length of the spring,
and F is a user defined force, possibly time
dependent.
While there are no TSDA elements in the spinning top, a zeros table of the appropriate
dimension is specified for consistent bookkeeping.
434
The Spatial RSDA Data Table (SRSDAT) is defined in lines 50 through 56. More
specifically, a template for TSDA T defined in Fig. 4.5.1, repeated here for clarity, is
SRSDAT(:,R)=[i;j;vxipr;vzipr;vxjpr;K;C;thet0;T];
Figure 4.5.3 Spatial RSDA

The user enters i, j, v ¢xi , v¢zi , v¢xj ,
K, C, thet0, and T, where K is
spring constant, C is damping
coefficient, thet0 is free angle
of the spring, and T is a user
defined torque, possibly time
dependent.
Finally, initial generalized coordinates and velocities are defined in lines 46 through 53.
If computation is required to define initial conditions that are consistent with constraints, code
doing so is entered in the main code, following line 47 in Fig. 5.9.1.
1 function [nb,ngc,nh,nhc,nd,nc,nv,nu,SJDT,SMDT,STSDAT,SRSDAT,...
2 NTSDA,NRSDA,q0,qd0]=AppData(app)
3
4 z3=[0;0;0];
5 z4=[0;0;0;0];
6 I3=eye(3);
7 ux=[1;0;0];
8 uy=[0;1;0];
9 uz=[0;0;1];
10
11 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
13
17 nhc=3; %Number of holonomic constraint equations, including drivers
18 nd=0; %number of driving constraints
20 nv=ngc-nc;
21 nu=nc;
24
25 ux=[1;0;0];
26 uy=[0;1;0];
27 uz=[0;0;1];
28 zer=zeros(3,1);
29
435
33 %6=Univ, 7=Strut, 8=Rev-Sph, 9=DistDr); i&j=bodies conn.,i>0;
35
36 SJDT(:,1)=[2;1;0;-uz;zer;0;zer;zer;zer;zer]; %Sph Jt - Body1 and ground
37
38 %SMDT(4,nb): Mass Data Table (With diagonal inertia matrix)
39 %SMDT=[[m1;J11;J12,J13],...,[mnb;Jnb1;Jnb2;Jnb3]]
40 SMDT=[30;90;90;30];
41
42 %STSDAT(12,1): TSDA Data Table
43 if NTSDA==0
44 STSDAT=zeros(12,NTSDA);
45 end
46 %STSDAT(:,T)=[i;j;sipr;sjpr;K;C;el0;F];
47 %T=TSDA No.; i&j=bodies conn.;si&jpr=vectors to Pi&j; K=spring constant;
48 %C=damping coefficient; el0=spring free length; F=const. force
49
50 %SRSDAT(15,1): RSDA Data Table
51 if NRSDA==0
52 SRSDAT=zeros(15,NRSDA);
53 end
54 %SRSDAT(:,R)=[i;j;vxipr;vzipr;vxjpr;K;C;thet0;T];
55 %R=RSDA No.; i&j=bodies conn.;si&jpr=vectors to Pi&j; K=spring constant;
56 %C=damping coefficient; thet0=spring free angle; T=const. torque
57
59 r10=[0;0;1];
60 p10=[1;0;0;0];
61 q0=[r10;p10];
62 omeg1pr0=[10^-12;10^-12;13.5];
63 r1d0=atil(omeg1pr0)*uz;
64 p1d0=0.5*GEval(p10)'*omeg1pr0;
65 qd0=[r1d0;p1d0];
66
67 end
Figure 5.9.2 AppData Function
Computational flow in the main program, which requires no input from the user beyond
line 92, is outlined in Fig. 5.9.3. Data required for integration that are defined in Section 5.9.1 are
initialized following line 95. Criteria for reparameterization, for both implicit and explicit
integrators, are defined following lines 129 and 153. Code to implement parameterization is
called following line 175, if error control tolerances are exceeded. Code that implements the
eight supported numerical integration methods is outlined following line 195. Integration results
for tangent space coordinates (v, vd, and vdd) that are computed using the selected integration
method are processed to obtain physical generalized coordinates (q, qd, qdd, and Lam) following
line 282. Total energy is computed following line 320. Output data of interest for each of the
436
applications discussed in Section 5.9.1 are defined following line 354. Finally, norms of position,
velocity, and acceleration constraint error are computed following line 406.
93 %Initialize Data for Integration
129 if integ<5 %Start implicit reparameterization criteria

153 if integ>4 %Start explicit reparameterization criteria
175 % Parameterization
195 % Integration
197 if integ==1 %Implicit ODE Trap
198 [v,vd,vdd,R1n,Jiter,JCond,Jinv,Jinviter,h,nch]= ...
199 ImplicitODETrap(n,tn,npar, ...
200 Vv,Vvd,Vvdd,Uu,q0,V,U,B,h,hmax,nch,SMDT,STSDAT,SJDT,par,InvJ,Jinv);
206 end
207
208 if integ==2 %Implicit ODE SDIRK54
209 [v,vd,vdd,Jiter,R1Norm,J,JCond,h,nch,Jinv,Jinviter]= ...
210 ImplicitODESDIRK54(n,tn,Vv,Vvd,Vvdd,Uu,q0,U,V,B,par, ...
211 h,hmax,nch,npar,SJDT,SMDT,STSDAT,Jinv,InvJ);
217 end
218
219 if integ==3 %Implicit Ind0 Trap
220 [v,vd,vdd,Lam,R1n,Jiter,JCond,h,nch]= ...
221 ImplicitInd0Trap(n,tn,npar,Vv,Vvd,Vvdd,LLam,Uu,q0,V,U,B,h,hmax,nch, ...
222 SMDT,STSDAT,SJDT,par);
229 end
230
231 if integ==4 %Implicit Ind0 SDIRK54
232 [v,vd,vdd,Lam,Jiter,R1n,J,JCond,h,nch]= ...
233 ImplicitInd0SDIRK54(n,tn,Vv,Vvd,Vvdd,LLam,Uu,q0,U,V,B,par, ...
234 h,hmax,nch,npar,SJDT,SMDT,STSDAT);
241 end
242
243 if integ==5 %Explicit ODE Nystrom4
244 [v,vd,vdd,ECond]=ExplicitODENystrom4(n,tn,Vv,Vvd,Uu,V,U,B,q0, ...
245 h,SMDT,STSDAT,SJDT,par);
247 end
248
249 if integ==6 %Explicit ODE RKFN45
250 [v,vd,vdd,ECond,h,nch]=ExplicitODERKFN45(n,tn,Vv,Vvd,Uu, ...
251 U,V,B,q0,h,hmax,par,SMDT,STSDAT,SJDT,nch);
255 end
256
257 if integ==7 %Explicit Ind0 Nystrom4
258 [v,vd,vdd,Lam,ECond]=ExplicitInd0Nystrom4(n,tn,Vv,Vvd,Uu, ...
259 V,U,B,q0,h,SMDT,STSDAT,SJDT,par);
263 end
264
265 if integ==8 %Explicit Ind0 RKFN45
266 [v,vd,vdd,Lam,ECond,h,nch]=ExplicitInd0RKFN45(n,tn,Vv,Vvd,Uu, ...
267 U,V,B,q0,h,hmax,par,SMDT,STSDAT,SJDT,nch);
437
272 end
274 %Process Results

282 %Evaluate q
292 %Update B and Evaluate qd
308 %Evaluate qdd and Lam
320 %Calculate Total Energy
submatrices to sparse matrices, below and to the right of the address of the 1-1 term in the
submatrix added to the underlying matrix that was initialized to zero. ATran evaluates the
orientation transformation matrix. Functions CorrectB and usolve evaluate the matrix B in Eq.
(5.2.22) and dependent generalized coordinates in Eq. (5.2.19). GamEval and GamsqqdEval
evaluate terms in Eq. (5.3.18) and Param evaluates terms required for the tangent space
parameterization of Section 5.2.
constraint and data table functions listed provide access to elements of the data tables that are
required to implement computation.
expressions and the numerous derivatives defined in Sections 3.2, 5.3, and 5.5 and Appendix 5.B
that are needed in implementing the tangent space ODE and Index 0 DAE formulations and
numerical integration of the associated equations of motion. The numerous bbxxx functions
implement building block constraint derivatives that underpin the physical joint constraint
equations. Care is taken to account for the fact that ground is designated by j = 0 and its
generalized coordinates are constant, yielding no derivative contribution, but including geometric
quantities that define the constraint of body i with ground. Similarly, the kinetic and derivative
evaluation functions listed evaluate inertial and generalized force terms and derivatives defined
in Sections 4.2, 5.3, and 5.5 and in Appendix 5.B that are required in formulation and solution of
the equations of motion.
Finally, numerical integration functions listed carry out the numerical integration process,
where ODEfunct evaluates tangent space ODE and Ind0ODEfunct evaluates tangent space Index
0 DAE of Sections 5.3 and 5.5, for use in the two explicit integrators. Similarly, JacobODE and
JacobInd0 evaluate terms in implicit numerical integration Jacobians that are defined in Sections
5.3 and 5.5, and ResidODE and ResidInd0 evaluate the residual of the equations of motion for
use in implicit integrators.
Internal details of the MATLAB functions listed in Fig. 5.9.4 are not presented, since
each of the functions is documented internally and the user need not modify these functions in
applications.
Computing Functions
Add
atil
438
EEval
GEval
KEval
BTran
ATran
CorrectB
usolv
GamEval
GamsqqdEval
Param
User Input Function
AppData
parPart
qPart
xPart
pNormPart
CylPart
DistPart
RevPart
TranPart
StrutPart
RevSphPart
UnivPart
STSDATPart
bbxxx
PhiEval
PhiqEval
P2Eval
P3Eval
P4Eval
P5Eval
MEval
M2Eval
QAEval
QAsqqd
ExplicitNystrom4
ExplicitRKFN45
ODEfunct
Ind0ODEfunct
ImplicitTrap
ImplicitSDIRK54
JacobODE
JacobInd0
ResidODE
ResidInd0
439
5.9.3 Code Output
reports the following arrays of values at each time step that shed light on the application and
performance of the code:
PosConstrNorm; norm of position constraint error
VelConstrNorm; norm of position constraint error
AccConstrNorm; norm of acceleration constraint error
Biterrpt; number of iterations required to update B
Iterurpt; number of iterations required to update u
hrpt; stepsize h for variable step integrators
ImpSoliterrpt; number of iterations required in implicit integration
JCondrpt; Condition number of matrix in implicit equation solution
jRepar; total number of reparameterizations in simulation
Q; array of values of q
Qd; array of values of q&
Qdd; array of values of q &&
Vv; array of values of v
Vvd; array of values of v&
Vvdd; array of values of &v&
LLam; array of values of Lagrange multiplier vector l
TE; total energy of system
Much as outlined in Section 5.7, the rudimentary Code 5.9 of Appendix 5.A is a tool for
numerical simulation and experimentation with the tangent space ODE and Index 0 DAE
formulations of spatial multibody dynamics. A general-purpose formulation and computer
implementation is especially important for spatial systems, due to the analytical complexity
associated with orientation of bodies and the resulting kinematic relations and equations of
motion. Development and use of such codes is the essence of the field of computational
mechanical system dynamics.
440
5.10 Spatial System Simulation Using Code 5.9
Six spatial examples are simulated using MATLAB Code 5.9 of Appendix 5.A.
Kinematic and kinetic characteristics of the models are defined and entered in AppData functions
defined in Section 5.9. Code 5.9 is then run and results analyzed. The technical complexity of ad-
hoc derivation of equations and associated coding, characteristic of examples presented earlier in
the chapter, is contrasted with the ease of use of a general-purpose computer code.
5.10.1 Spin Stabilized Top
The spin stabilized top studied in Sections 5.1 and 5.4.2.1 is simulated using Code 5.9,
with the AppData function of Fig. 5.10.1.

12
18 nv=ngc-nc;
19 nu=nc;
21
22 ux=[1;0;0];
23 uy=[0;1;0];
24 uz=[0;0;1];
25 zer=zeros(3,1);
26
27 %SJDT(22,nh): Joint Data Table
28 %SJDT(:,k)=[t;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr];
29 %k=joint No.; t=joint type(1=Dist,2=Sph,3=Cyl, 4=Rev, 5=Tran,
30 %6=Univ, 7=Strut, 8=Rev-Sph); i&j=bodies conn.,i>0;
32 SJDT(:,1)=[2;1;0;-uz;zer;0;zer;zer;zer;zer]; %Sph Jt - Body1 and ground
33
36 SMDT=[30;90;90;30];
37
39 if NTSDA==0
41 end
45
47 r10=[0;0;1];
48 p10=[1;0;0;0];
49 q0=[r10;p10];
441
50 omeg1pr0=[10^-12;10^-12;13.5];
51 r1d0=atil(omeg1pr0)*uz;
52 p1d0=0.5*GEval(p10)'*omeg1pr0;
53 qd0=[r1d0;p1d0];
54
55 end
Figure 5.10.1 AppData Function, Spin Stabilized Top
Plots of x vs y coordinates of the centroid of the top presented in Fig. 5.10.2 were
obtained using Code 5.9 with the trapezoidal integrator and ODE formulation and Atol = e-5.
These results are essentially identical to those presented in Sections 5.1 and 5.4.2.1. Maximum
position, velocity, and acceleration constraint error norms over the 100 sec simulation were 1.4e-
14, 3e-15, and 1.8e-13, respectively. The maximum variation in total energy for the conservative
system was 0.005%. Essentially identical results were obtained with the other 7 integrator-
formulation combinations supported in Code 5.9.
Figure 5.10.2 Centroidal x vs y for 100 sec Simulation of Spin Stabilized Top
442
The spatial double pendulum of Section 5.4.4 is simulated using Code 5.9, with the
58 if app==2 %Spatial Double Pendulum

65 nv=ngc-nc;
66 nu=nc;
69 ux=[1;0;0];
70 uy=[0;1;0];
71 uz=[0;0;1];
72 zer=zeros(3,1);
73
75 %SJTd(:,k)=[t;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr];
79 SJDT(:,1)=[2;1;0;zer;zer;0;zer;zer;zer;zer]; %Sph.-Body 1 to Ground
80 SJDT(:,2)=[1;1;2;-uz;uz;1;zer;zer;zer;zer]; %Dist. - Body 1 to 2
81
84 SMDT=[[75;30;30;30],[75;30;30;30]];
85
87 if NTSDA==0
89 end
91 %T=TSDA No.; i&j=bodies conn.;si&jpr=vectors to Pi&j; K=spr const;
93
95 r10=[0;0;0];
96 p10=[1;0;0;0];
97 r20=[0;0;-3];
98 p20=[1;0;0;0];
99 q0=[r10;p10;r20;p20];
100 r1d0=[0;0;0];
101 p1d0=[0;0;0;0];
102 r2d0=[0;0;0];
103 p2d0=0.5*GEval(p20)'*[0;5;0];
104 qd0=[r1d0;p1d0;r2d0;p2d0];
Figure 5.10.3 AppData Function, Spatial Double Pendulum
443
The top plot of numerical results in Fig. 5.10.4, using the trapezoidal integrator with the
ODE formulation and Atol = e-5, is for the pendulum with a distance constraint d = 1 m. Results
are essentially identical to those presented in Fig. 5.4.10. This simulation satisfied the same
integration error tolerances as used in Section 5.4.4, with the same integrator, and satisfied
kinematic constraints to the same level of precision reported in Table 5.4.4. Essentially identical
results were obtained using the other 7 integrator-formulation alternatives supported in Code 5.9.
Figure 5.10.4 x2 Coordinate of Centroid vs Time for Spatial Double Pendulum

The bottom plot in Fig. 5.10.4 is for the same pendulum, but with the variable distance
constraint d = d(t) = 1 + 0.05sin(10t) , which is entered in the function PhiEval for evaluation of
the distance constraint. The constraint time derivatives
F t = 0.5 (1 + 0.05sin(10t) ) cos(10t)
F tt = -5 (1 + 0.05sin(10t) ) sin(10t) + 0.25cos 2 (10t)
are entered in the P5 function for evaluation of time dependent terms associated with the distance
constraint. Clearly, this distance constraint, with a time dependent 5% variation in d, has a
significant influence on the solution.
As observed in Section 5.8, use of the general purpose Code 5.9 with the data set of Fig
5.10.3 provides accurate simulation results in a matter of minutes of person time to populate the
data set and carry out simulations on a laptop workstation. This is contrasted with the intricate
expressions in Section 5.4.4 that required several person days for derivation and creation and
444
debugging of the ad-hoc computer Code 5.4.4 of Appendix 5.A. MATLAB timing results
reported 140 CPU sec for the 20 sec simulation with the general-purpose Code 5.9 vs 76 CPU
sec for simulation with the ad-hoc Code 5.4.4. The saving of one minute of CPU time on a laptop
workstation hardly compensates for the time consuming and painful process of ad-hoc derivation
and programming reported in Section 5.4.4.
5.10.3 Body in Cylindrical Joint with Ground
A single body i = 1 shown in Fig. 5.10.5 is constrained to rotate and translate in a
cylindrical joint with axis along the global z axis in nonmoving ground body j = 0. A spring
connects point A (0,1,1) on the body with point B (-1,1,1) in ground. With joint reference frames
initially at the origin of and aligned with the global x-y-z frame, s¢01 = s1¢1 = 0 ,
v¢x1 = u x , v¢z1 = u z , v¢x2 = u x , and v¢z2 = u z . With ground fixed in space, r0 = 0 and
p0 = [1 0 0 0] . The initial position and orientation of body 1 are
T
r1 (0) = 0 and p1 (0) = [1 0 0 0 ] , as shown in Fig. 5.10.5. Initial velocity and angular velocity
T
are r&1 (0) = 0 and omeg(0) = 10 rad/sec. The mass and moments of inertia of body 1 are 1 kg and
1 kg × m2 about each axis, gravitational acceleration is 9.8 m/ sec 2 in the negative z direction, the
free length of the spring is l 0 = 1 m, and the spring constant is k = 100 n/m.
Figure 5.10.5 Body in Cylindrical Joint with Ground

A simulation is carried out using Code 5.9, with data defined in the AppData function of
Fig. 5.10.6, the trapezoidal ODE formulation, an absolute error tolerance Atol = e-5. The
resulting vertical position and velocity and angular velocity about the z axis are shown in Fig.
5.10.7. The error control mechanism led to an average step size of 7.7e-4 sec and a single
parameterization covered the 6,496 step simulation. Maximum position, velocity, and
acceleration constraint norms were 6e-11, 3.5e-9, and 1.2e-7, respectively. Essentially identical
results were obtained with each of the other seven formulations and numerical integrators
supported in Code 5.9, with reparameterizations ranging from one to 17.
108 if app==3 %One Body in Cylindrical with Spring

109
445
C:\Users\echau\Documents\CMMD\Chapter 5. Tangent Space Methods for H...\AppData.m Page 3
115 nv=ngc-nc;
116 nu=nc;
118
119 ux=[1;0;0];
120 uy=[0;1;0];
121 uz=[0;0;1];
122 zer=zeros(3,1);
123
129 SJDT(:,1)=[3;1;0;zer;zer;0;ux;uz;ux;uz]; %Cyl - Body1 to Ground
130
133 SMDT=[[1;1;1;1]];
134
136 if NTSDA==0
138 end
142 STSDAT(:,1)=[1;0;uy+uz;-ux+uy+uz;100;0;1;0];
143
145 r10=[0;0;0];
146 p10=[1;0;0;0];
147 q0=[r10;p10];
148 r1d0=[0;0;0];
149 p1d0=0.5*EEval(p10)'*10*uz;
150 qd0=[r1d0;p1d0];
151
152 end
Figure 5.10.6 AppData Function, Body in Cylindrical Joint with Ground
446
Figure 5.10.7 Vertical Position, Velocity, and Angular Velocity
5.10.4 Spatial Slider-Crank
The spatial slider-crank mechanism shown in Fig. 5.10.8, whose kinematics were studied
in Section 3.12.5, consists of two bodies and a distance constraint to model the connecting rod
between them. It involves potentially large constraint contact forces and a near singular
configuration. The crank (body 1) rotates abut an axis parallel to and offset from the global z
axis in ground (body 0), with a crank in the x-y plane of radius 0.08 m. The slider (body 2)
translates in a guide along the global z axis in ground. It has an offset of length c 2 in the positive
z2 direction for distance constraint attachment, which increases constraint reaction forces in the
translational joint. A distance constraint of length d 3 connects the top of the offset in body 2
with the end of the crank in body 1. Data are given in the mks system of units.
447
Since the x ¢-y ¢-z ¢ reference frames in both bodies and ground are parallel in the initial
configuration shown, unit vectors used in definition of the joints are v¢x = uz , v¢y = u y , and
ik ik
v¢zik = ux , i = 0, 1, 2 and k = 1 and 2. For the revolute joint, s1¢1 = 0 , s¢01 = [ -1 0.1 0.12 ]T . For the
translational joint, s¢2 = s¢0 = 0 . For the distance constraint, s1¢3 = 0.08uz , c2 = 0.02 , s¢2 = c2u y ,
2 2 3
and d 3 = 0.4 . Masses and moments of inertia are m1 = 0.5 and Iαα = 0.2 for body one and
m2 = 5 and Iββ = 0.2 for body two. Initial conditions are r10 = [0, 0.1, 0.12]T , r20 = [0.3429, 0, 0]T ,
p10 = p02 = [1,0,0, 0]T , r&10 = [0, 0, 0]T , r&20 = [2.799, 0, 0]T . Angular velocity of body 1 about the x
axis is 120 rad/sec, so p& 10 = 0.5E(p10 )T120u x and p& 02 = [0, 0, 0, 0]T . The values of z2 and z& 2 were
estimated and initial simulations run for tf = 0.002, adjusting the values until the variable arrays
Q and Qd are consistent with initial conditions. Data for simulation, using Code 5.9 of Appendix
5.A, are presented in the AppData function of Fig. 5.10.9.
155 if app==4 %Spatial Slider-Crank

156
162 nv=ngc-nc;
163 nu=nc;
165
166 ux=[1;0;0];
167 uy=[0;1;0];
168 uz=[0;0;1];
169 zer=zeros(3,1);
170
448
176 SJDT(:,1)=[4;1;0;zer;0.1*uy+0.12*uz;0;uz;ux;uz;ux]; %Rev-Body1 to Ground
177 SJDT(:,2)=[5;2;0;zer;zer;0;uz;ux;uz;ux]; %Tran-Body2 to Ground
178 SJDT(:,3)=[1;1;2;0.08*uz;0.02*uz;0.3;zer;zer;zer;zer]; %Dist-Body 1 to 2
179
182 SMDT=[[0.5;0.2;0.2;0.2],[5;0.2;0.2;0.2]];
183
185 if NTSDA==0
187 end
191
193 r10=[0;0.1;0.12];
194 p10=[1;0;0;0];
195 r20=[0.2182;0;0];
196 p20=[1;0;0;0];
197 q0=[r10;p10;r20;p20];
198 r1d0=[0;0;0];
199 p1d0=0.5*EEval(p10)'*120*ux;
200 r2d0=[4.406;0;0];
201 p2d0=[0;0;0;0];
202 qd0=[r1d0;p1d0;r2d0;p2d0];
203
204 end
Numerical results for angular velocity omega1x and angular acceleration omega1xd of
the crank for the data set in Fig. 5.10.9 with d 3 = 0.3 are shown in the bottom plots of Fig.
5.10.10. The effect of the massive slider on angular velocity and acceleration is clear. Results for
d 3 = 0.23 (with x 02 = 0.1025 and x& 02 = 9.369 ) in the top plots show more extreme angular
velocities and especially angular accelerations of the crank. This is due to their approach toward
a singular configuration at d 3 = 0.22.
Even for the most extreme simulation, the maximum total energy variation is 0.0017%
and the maximum position, velocity, and acceleration error norms over the simulation are e-9,
2e-11, and 2e-8, respectively. These results are obtained using the trapezoidal integrator with the
ODE formulation and Atol = e-5. Essentially identical results are obtained using the other 7
integrator-formulation alternatives supported in Code 5.9.
449
Figure 5.10.10 omega1-x and omega1-xd vs t for Spatial Slider-Crank
The ease with which this system model was created using MATLAB Code 5.9 in about
two person hours and accurate results obtained in an 8 CPU sec simulation on a laptop
workstation belies the extreme agony that would otherwise be experienced if ad-hoc equation
derivation were carried out, and even more pain that would be involved in writing and debugging
ad-hoc computer code.
5.10.5 Four-Body Mechanism with Translational Joints in Ground
The four-body mechanism test problem shown in Fig. 5.10.11 is comprised of four bodies
that translate along the coordinate axes, connected by distance constraints and springs. The
model employs 28 generalized coordinates and a total of 26 constraint equations, for two degrees
of freedom. Body reference frames in each of the four bodies are parallel to the global reference
frame, leading to the constraint data v¢xi = u z and v¢zi = u x , i = 1, 0, 4, 0 ,
v¢xi = u z and v¢zi = u y , i = 2, 0 , and v¢xi = u x and v¢zi = u z , i = 3, 0 .
Figure 5.10.11 Four Body Mechanism with Translational Joints in Ground
450
The full AppData function for the simulation using Code 5.9 is given in Fig. 5.10.12. Simulations
with error control parameters intol =e-6 and Atol = e-5 yield position and velocity plots presented
in Fig. 5.10.13.
207 if app==5 %4-mass Spatial Model
208
214 nv=ngc-nc;
215 nu=nc;
217
218 ux=[1;0;0];
219 uy=[0;1;0];
220 uz=[0;0;1];
221 zer=zeros(3,1);
222
229 SJDT(:,2)=[5;2;0;zer;zer;0;uz;uy;uz;uy]; %Tran-Body2 to Ground
230 SJDT(:,3)=[5;3;0;zer;zer;0;ux;uz;ux;uz]; %Tran-Body3 to Ground
232 SJDT(:,5)=[1;1;2;zer;zer;5;zer;zer;zer;zer]; %Dist-Body 1 to 2
233 SJDT(:,6)=[1;2;3;zer;zer;7;zer;zer;zer;zer]; %Dist-Body 2 to 3
234
237 SMDT=[[2;0.2;0.2;0.2],[2;0.2;0.2;0.2],[6;0.2;0.2;0.2],[6;0.2;0.2;0.2]];
238
240 if NTSDA==0
242 end
246 STSDAT(:,1)=[1;0;10*ux;zer;2;0;14;0]; %mass1 to ground
247 STSDAT(:,2)=[2;0;10*uy;zer;2;0;13;0]; %mass2 to ground
248 STSDAT(:,3)=[3;0;10*uz;zer;2;0;16.32;0]; %mass3 to ground
249 STSDAT(:,4)=[1;4;zer;10*ux;10;0;11;0]; %mass4 to mass1
250
252 r10=[4;0;0];
253 p10=[1;0;0;0];
254 r20=[0;3;0];
451
255 p20=[1;0;0;0];
256 r30=[0;0;6.32];
257 p30=[1;0;0;0];
258 r40=[5;0;0];
259 p40=[1;0;0;0];
260 q0=[r10;p10;r20;p20;r30;p30;r40;p40];
261 r1d0=[0;0;0];
262 p1d0=[0;0;0;0];
263 r2d0=[0;0;0];
264 p2d0=[0;0;0;0];
265 r3d0=[0;0;0];
266 p3d0=[0;0;0;0];
267 r4d0=[-1;0;0];
268 p4d0=[0;0;0;0];
269 qd0=[r1d0;p1d0;r2d0;p2d0;r3d0;p3d0;r4d0;p4d0];
Fig. 5.10.12 AppData Function, Spatial Four Body Mechanism
Figure 5.10.13 Positions and Velocities of Four Bodies

While the model is of higher dimension than previous examples, all eight formulations
and integration algorithms supported in Code 5.9 perform comparably. Time steps taken are at
the maximum allowable hmax = e-3, 2 to 5 reparameterizations were sufficient in all cases, and
maximum constraint position, velocity, and acceleration constraint error norms were 1.5e-11, 4e-
9, and 2e-8, respectively.
The disk (body 1) in Fig 5.10.14, as in the planar model of Fig. 5.8.20, rotates in ground
(body 0) about the global z axis through the origin of the x-y frame. Body 2 translates in body 1
along a guide that is parallel to the body fixed y1¢ axis, one unit to the right of the y1¢ axis. A
TSDA with spring constant K = 10 N/m, damping constant C = 0 Nsec/m, and free length l 0 =
10.1 m acts between the origin of the x 2 -y2 reference frame and a point u1x¢ + 10u1y¢ in body 1.
The initial conditions for the system are r10 = [ 0 0 0 ] , p10 = [1 0 0 0 ] q& 10 = 0 ,
T T
r20 = [1 0 0 ] , p 02 = [1 0 0 0 ] , and q& 02 = 0 . The masses and moments of inertia of the

T T
bodies are m1 = 10 kg, J aa = 10 kg × m 2 , m 2 = 5 kg, and J bb = 5 kg × m 2 .
452
The AppData function for this system in Code 5.9 is presented in Fig. 5.10.15. Numerical
results for rotation of the disk and translation of body 2 relative to body 1 for simulations without
and with damping are identical to those presented in Figs. 5.8.22 and 5.8.23 for the planar model
of Section 5.8.6 that encompasses the same mechanics. Identical results are obtained with all
eight integration formulations, for both data sets.
274 if app==6 %Rotating Disk with Translating Body

275
281 nv=ngc-nc;
282 nu=nc;
284
285 ux=[1;0;0];
286 uy=[0;1;0];
287 uz=[0;0;1];
288 zer=zeros(3,1);
289
295 SJDT(:,1)=[4;1;0;z3;z3;0;ux;uz;ux;uz]; %Rev. - Body 1 to Ground
296 SJDT(:,2)=[5;1;2;ux;z3;0;ux;uy;ux;uy]; %Tran. - Body 1 to 2
297
300 SMDT=[[10;10;10;10],[5;5;5;5]];
301
303 if NTSDA==0
453
305 end
309 STSDAT(:,1)=[1;2;ux+10*uy;z3;10;0;10.1;0];
310
312 r10=[0;0;0];
313 p10=[1;0;0;0];
314 r20=[1;0;0];
315 p20=[1;0;0;0];
316 q0=[r10;p10;r20;p20];
317 r1d0=[0;0;0];
318 p1d0=[0;0;0;0];
319 r2d0=[0;0;0];
320 p2d0=[0;0;0;0];
321 qd0=[r1d0;p1d0;r2d0;p2d0];
322 end
Fig. 5.10.15 AppData Function, Rotating Disk with Translating Body
Analysis of spatial multibody systems using Code 5.9 of Appendix 5.A confirms that
properties of the tangent space ODE and Index 0 DAE formulations and four numerical
integration methods in generating and solving the equations of motion are as demonstrated in
planar applications with Code 5.7. The effectiveness of using a general-purpose code for
spatial applications is amplified by the complexity that would otherwise be encountered in ad-
hoc derivation and coding of spatial equations of motion, such as those presented in Sections
5.4 and 5.6.
454
5.11 Well Posed Formulations of Holonomic Mechanical System Dynamics
In preparation for treatment of the Full DAE of Section 4.10.2 in Chapter 7 and friction
effects in Chapter 8, existence, uniqueness, and regularity of solutions of equations of motion
must be established. The result presented in Section 5.2.4 that the tangent space ODE is well
posed is extended in this section, to include the tangent space Index 0 DAE, Full DAE, and
d’Alembert variational formulations. To realize this objective, dependence of system kinematic
and dynamic equations on design variables, the primary form of problem data considered, is
accounted for and an abbreviated derivation of the tangent space ODE, as a function of design
variables, is presented. Theorems that establish well posedness of each of the formulations
treated are presented and proved.
5.11.1 Introduction
As stated in the ODE existence theorem of Section 4.7.3, a well posed problem is defined
as one for which there exists a unique solution that depends continuously on problem data. The
existence and uniqueness components of the definition are broadly embraced in the mathematical
literature, but the issue of continuous dependence on problem data receives less attention. In the
field of mechanical system dynamics, well posedness is most often invoked for systems whose
dynamics are represented by explicitly defined ODE initial-value problems. For holonomic
systems that are modelled using the Full DAE, as in Section 4.10.2, existence and uniqueness
results are available (Rheinboldt, 1991), but continuous dependence on data remains to be
established. For systems whose dynamics are represented by d’Alambert’s form of variational
equations of motion in Section 4.6, the concept of well posedness is seldom addressed.
Recent work on dynamic system design sensitivity analysis (DSA) (Callejo and Garcıa de
Jalon,2014; Dopico, Zhu, Sandu, and Sandu, 2015; Banerjee and McPhee, 2016), which relies on
differentiability of dynamic response with respect to design variables, has used Maggi and Kane
ODE formulations of system dynamics, assuming they are well posed. As shown in Appendix
5.D, these formulations are subject to errors due to failure to enforce configuration constraints
(Haug, 2019d), and may not be well posed.
Design dependence in the d’Alembert variational formulation of the equations of
mechanical system dynamics is introduced in Section 5.11.2 and associated dependence of initial
conditions on design variables is analyzed in Section 5.11.3. Tangent space ODE of system
dynamics that account for design dependence are derived in Section 5.11.4. It is shown in
Section 5.11.5 that the tangent space ODE are well posed and equivalent to the Index 0 DAE
formulation, which is also shown to be well posed. It is proved in Sections 5.11.6 and 5.11.7 that
the variational and Full DAE formulations are well posed.
5.11.2 Design Dependence in Variational Equations of Motion
The mechanical systems treated are defined as collections of rigid bodies whose position
and orientation relative to an inertial reference frame are defined by generalized coordinates
q = [q1 L q n ] Î R ngc that satisfy holonomic constraints of the form
T
Φ( q, b) = [F1 ( q, b ) L F nhc ( q, b) ] = 0
T
(5.11.1)
455
where b = [ b1 L b k ] Î R k is a vector of problem data, or design variables, that do not
T
depend on time and define the geometry and kinetics of the system. Time independent
constraints are treated here, but the formulation may be extended to time dependent constraints,
as in Sections 5.2 and 5.5. Differentiating Eq. (5.11.1) with respect to time yields velocity and
acceleration constraints,
Φ q (q, b )q& = 0 (5.11.2)
Φ q (q, b)q ( )
&& + Φ q (q, bˆ )qˆ& q& º Φ q (q, b)q
q
&& + γ (q, q& , b ) = 0 (5.11.3)
It is important that the Jacobian Φ q (q, b ) of the holonomic constraints has full row rank, for all
q and b in neighborhoods X 0 Ì R ngc of q 0 = q(t 0 ) and B0 Ì R k of b.
Dynamics of the system is governed by d’Alembert’s principle of Section 4.6,
dq T ( M(q, b)q
&& - Q A (q, q& , b, t) - S(q, q& , b ) ) = 0 (5.11.4)
for b Î B0 , which must hold for all dq that satisfy
Φq (q, b)dq = 0 (5.11.5)
Hypotheses for mechanical systems considered are that the mass matrix M((q, b ) is positive
definite on the null space of Φ q (q, b ) , Φq (q, b) has full row rank, QA (q, q& , b,t) is a vector of
generalized applied forces, S(q , q& , b ) is a vector of velocity coupling terms, and all functions that
appear in Eqs. (5.11.1) through (5.11.4) are at least continuously differentiable with respect to
q, q& , and b . Consistent with d’Alembert’s principle, no forces of constraint appear in Eq.
(5.11.4).
5.11.3 Initial Conditions as Functions of Problem Data
At initial time t 0 , Eq. (5.11.1) must hold; i.e., Φ(q0 ,b) = 0 . In addition to these nhc
conditions, ngc - nhc initial conditions on q must be imposed, in the form
Ψ(q0 , b) = 0 (5.11.6)
where Ψ Î R ngc- nhc . The combined initial conditions are
é F( q 0 , b ) ù
ê ú=0 (5.11.7)
ë Y( q , b ) û
0
and the associated Jacobian,

0 ˆ
é Φ q0 ù éΦ(q ,b) q0
ù é Φ (q0 ,b) ù
ê ú= ê ú =ê
q
ú
Ψ ˆ Ψ (q0 ,b)ú
ëê q0 ûú êë Ψ(q ,b)q0 úû ëê q0
0
û
456
Is required to have full rank. As long as q 0 satisfies Eq. (5.11.7), the implicit function theorem
of Section 2.2.5 implies that Eq. (5.11.7) uniquely determines the initial configuration as a
continuously differentiable function of b; i.e., q(t 0 , b) = q0 = q0 (b) , for all b Î B0 .
Initial conditions on q& are generally ngc - nhc linear equations of the form
Θ(q0 (b),b)q& 0 = α(b) (5.11.8)
where the coefficient matrix Θ(q0 (b ), b) and vector α(b ) are continuously differentiable
functions of q 0 and b. This includes initial conditions in spatial systems that are stated in terms
of angular velocity. Combining Eq. (5.11.2), evaluated at t 0 , with Eq. (5.11.8),
éΦ q (q 0 ( b ), b ) ù 0 é 0 ù
ê ú q& (b ) = êα (b ) ú (5.11.9)
ë Θ(q (b ),b) û
0
ë û
The coefficient matrix of Eq. (5.11.9) must be nonsingular to uniquely determine the initial
velocity as a continuously differentiable function of b; i.e., q& (t 0 , b ) = q& 0 = q& 0 (b ) , for all b Î B0 .
Accounting for design dependence of initial conditions is a critical component of DSA.
Neglecting design dependence of initial conditions leads to serious errors in results. In particular,
even if initial configuration or velocity conditions of Eqs. (5.11.6) and (5.11.8) do not depend
explicitly on b, Eqs. (5.11.7) and (5.11.9) depend on b, through the kinematic constraint
equations and initial conditions depend on b.
5.11.4 ODE of Motion
Derivation of design dependent tangent space ODE is summarized in this section, as an
extension of the development of Section 5.2. At initial time t 0 , U(b) is defined as the ngc ´ nhc
matrix
U (b ) º Φ Tq ( q 0 (b ), b ) (5.11.10)
which is continuously differentiable with respect to b and has full rank. A second matrix V(b) is
defined at t 0 as the solution of
Φ q (q0 (b ), b )V = U(b )T V = 0
(5.11.11)
V V=I
T
using singular value decomposition (Strang, 1980). The second of Eqs. (5.11.11) implies that
V(b ) has full rank, for all b Î B0 . Further, since the rank of Φq (q0 (b ), b) is nhc, V is an
ngc ´ (ngc-nhc) matrix (Strang, 1980). Since its columns are orthogonal to the columns of U(b ) ,
the columns of U(b ) and V(b ) span R ngc , for each b Î B0 . The Jacobian of Eqs. (5.11.11) with
respect to V is
éΦ 0q ( q, b ) ù é U (b ) T ù
ê ú=ê Tú
ë 2 V (b ) û ë2 V (b ) û
T
457
Since the columns of U(b ) and V(b ) are linearly independent, the rows of U(b)T and V(b)T
are linearly independent. Thus, Eq. (5.11.11) has a unique solution V = V (b ) that is
continuously differentiable with respect to b, for all b Î B0 .
As in Eq. (5.2.7), matrices V(b ) and U(b ) are further related by the identity
V(b)V(b )T + U (b )( U(b )T U(b))-1 U (b )T = I (5.11.12)
in b; i.e., it holds for all b Î B0 .
Since every vector in R ngc can be represented as a unique linear combination of columns
of V( b ) and U( b) , a tangent space parameterization of system kinematics that accounts for
design dependence is
q = q 0 ( b ) + V ( b ) v - U ( b )u (5.11.13)
At t 0 , q(t 0 , b ) = q 0 (b ) , so Eq. (5.11.13) reduces to V(b)v0 - U(b)u0 = 0 . Multiplication on the

left by VT (b ) and UT (b) , using Eqs. (5.11.11) and the fact that U T (b)U(b) is nonsingular,
yields
v0 = 0
(5.11.14)
u0 = 0
which do not depend on b.
For q of Eq. (5.11.13) to satisfy the constraints of Eq. (5.11.1), it is required that
(
Φ ( q 0 (b ) + V( b ) v - U( b )u ) , b = 0 ) (5.11.15)
The partial derivative of the left side of this equation with respect to u is
( (( ˆ ˆ - Uu
Φ qˆ 0 + Vv ) ))
ˆ , bˆ = -Φ (q, b)U(b) . At t 0 , this is
u
q
Φ u ( q 0 (b ), b ) = -Φ q ( q 0 (b ), b ) U ( b ) = -U (b )T U (b ) º -( B 0 (b )) -1 (5.11.16)
which is nonsingular. Since Fq (q,b) and U(b) are continuous functions of q and b,
B( q , b ) º ( Φ q (q , b ) U ( b ) )
-1
(5.11.17)
is nonsingular and continuously differentiable with respect to q and b in a neighborhood (X0 ,B0 )
of (q0 (b), b) . Since Φu (q , b ) is nonsingular in a neighborhood of (q 0 (b ), b) and u = 0 is a
solution of Eq. (5.11.15) with v = 0 , the implicit function theorem of Section 2.2.5 guarantees
that Eq. (5.11.15) has a unique solution,
u = h( v, b) (5.11.18)
that is continuously differentiable with respect to v and b in a neighborhood ( V0 ,B0 ) of ( v0 , b) .

Thus, Eq. (5.11.13) is
q( v, b) = q0 (b) + V(b)v - U(b)h( v, b) (5.11.19)
458
which satisfies Eq. (5.11.1) for all v and b in a neighborhood ( V0 ,B0 ) of ( v0 , b) . As in Section
5.2, B(q, b ) and h (q, b ) can be efficiently evaluated numerically, for each b Î B0 , but the
magnitude of v and condition number of B( q, b ) must be monitored to assure the
parameterization of Eq. (5.11.9) remains valid.
Differentiating Eq. (5.11.19) with respect to v,
q v = V(b) - U(b)h v (v, b) (5.11.20)
To evaluate h v (v, b) , note that Eq. (5.11.15) with b constant and u = h( v, b) is an identity in
v Î V0 . Its derivative with respect to v, suppressing arguments, is Φ q ( V - Uh v ) = 0 . Using Eq.
(5.11.17),
h v (v, b ) = B(q, b )Φq (q, b )V (b ) (5.11.21)
which is a continuously differentiable function of q and b and Eq. (5.11.20) reduces to

q v = V (b ) - U(b )B(q, b)Fq (q, b )V (b ) = D(q, b) (5.11.22)
where
D(q, b ) º ( I - U( b )B(q, b )Φ q ( q, b) ) V(b) (5.11.23)
Thus,
q& = qv v& = D(q, b )v& (5.11.24)
At t 0 , multiplying Eq. (5.11.24) on the left by V(b )T and using the facts that
V(b)T U(b) = 0 and D(q0 , b ) = V(b) - U(b)Φq (q0 , b )V(b ) = V(b) ,
v& 0 (b) = V(b)T q& 0 (b) (5.11.25)

An important property of D(q, b) is obtained using Eq. (5.11.17),
Φ q D = ( Φ q - Φ q UBΦ q ) V = ( Φ q - Φ q ) V = 0 (5.11.26)
This shows that D is in the null space of the constraint Jacobian, for all b Î B0 . Multiplying Eq.
(5.11.24) on the left by the Jacobian Φq , Φq q& = Φq Dv& = 0 , so q& of Eq. (5.11.24) satisfies the
velocity constraint equation of Eq. (5.11.2), for all v Î V0 , b Î B0 , and v& .
Differentiating Eq. (5.11.24) with respect to v& and v,
q& v& = D(q, b) (5.11.27)
( ) (
q& v = D(q, bˆ )v& q v = D(q, bˆ )v& D(q, b )
q
) q
(5.11.28)
From Eq. (5.11.23),
( D(q, bˆ )aˆ ) q
(
= -U B(q, bˆ )Φ
ˆ Va
q
ˆˆ ) q
(
- UB Φq (q, bˆ )Va
ˆˆ ) q
(5.11.29)
459
(
To evaluate B(q, bˆ )Φ
ˆ Va
q )
ˆ ˆ , Eq. (5.11.17) is written in the form Φ (q, b )UB(q, b )cˆ = cˆ , which
q
q
is an identity in q. Differentiating with respect to q, Φq (q, bˆ )UBc

ˆ ˆˆ ( ) q
(
+ Φ q U B(q, bˆ )cˆ ) q
= 0.
Using Eq. (5.11.17), this is
( B(q, bˆ )cˆ ) q
(
= - B Φ q (q, bˆ )UBc
ˆ ˆˆ ) q
(5.11.30)
Substituting this result, with c = Φq Va , into Eq. (5.11.29) ,
( D(q, bˆ )aˆ ) q
(
= UB Φq (q, bˆ )UBΦ
ˆ ˆ ˆ Va
q
ˆˆ ) q
(
- UB Φq (q, bˆ )Va
ˆˆ ) q
( ) )
(5.11.31)
= -UB Φq (q, bˆ ) I - UBΦ
ˆ ˆ ˆ Va
q (
ˆˆ
q
(
= -UB Φq (q, bˆ )Da
ˆˆ ) q
Using Eqs. (5.11.24) and (5.11.31), Eq. (5.11.28) reduces to
(
q& v = - UB Φ q (q, bˆ )Dv )
ˆ &ˆ D = - UB Φ (q, bˆ )q&ˆ D
q
q
( ) q
(5.11.32)
q = q& v& &&

Using Eqs. (5.11.27) and (5.11.32) with && v + q& v v& ,
&& v - UBγ ( q, q& , b )
q = D(q, b )&& (5.11.33)
( )
where Φ q (q, bˆ )q&ˆ q& º γ (q, q,b
q
& ) of Eq. (5.11.3) has been used.
Multiplying both sides of Eq. (5.11.33) on the left by Φ q (q, b ) , suppressing arguments
q = Φq Dv
and using Eqs. (5.11.17) and (5.11.33), Φq && && - Φq UBγ = - γ . Thus, the acceleration
constraint of Eq. (5.11.3) is satisfied for all v Î V0 , b Î B0 , v& , and &&
v . In summary, all three forms
of constraint of Eqs. (5.11.1) through (5.11.3) are satisfied by q, q& , and && q of Eqs. (5.11.19),
(5.11.24), and (5.11.33), for all v Î V0 , b Î B0 , v& , and &&
v.
In differential form, Eq. (5.11.24) is dq = D(q, b )dv and, since Φq (q, b )D(q, b ) = 0 ,
Φq (q, b)dq = Φq (q, b )D(q, b )dv = 0 (5.11.34)
for arbitrary dv . Substituting this result and q, q& , and q

&& of Eqs. (5.11.19), (5.11.24), and
(5.11.33) into Eq. (5.11.4) yields
ì D T (q(v,b ),b )M (q(v,b ),b )D(q(v,b ),b )&& v ü
ï ï
dv T í - D T (q(v,b ),b )M(q(v ,b ),b )U (b )B(q(v,b ),b )γ (q(v,b ), q& (v, v& ,b ),b ) ý=0
ï - D T (q(v,b ),b )Q A (q (v,b ), q& (v, v& ,b ), b, t) - D T (q (v,b ),b )S(q(v,b ), q& (v, v& ,b ),b ) ï
î þ
(5.11.35)
Since dv is arbitrary,
460
DT (q(v,b ),b)M(q(v,b),b )D(q(v,b ),b)&&
v
- DT (q(v,b ),b)M(q(v,b ),b )U (b )B(q(v,b),b )γ (q(v,b ), q& (v , v& ,b ),b) (5.11.36)
- D (q(v,b ),b)Q (q(v,b ), q& (v, v& ,b ), b, t) - D (q(v,b ),b )S(q(v,b ), q& (v, v& ,b),b ) = 0
T A T
which is a second order ODE in v that depends on b.

Evaluating Eq. (5.11.13) at t 0 and multiplying by V T , 0 = VT (q0 - q 0 ) = v0 . This and
Eq. (5.11.25) yield the design dependent initial conditions
v(t 0 ) = 0
(5.11.37)
v& (t 0 ) = V(b)Tq& 0
5.11.5 Well Posed ODE and Index 0 DAE of Motion
By the hypotheses of Section 5.11.2,
DT (q0 ,b )M(q0 ,b )D(q0 ,b ) = Φq (q 0 ,b)M(q0 ,b )ΦqT (q0 ,b )
is positive definite. Since DT (q, b )M(q, b )D(q, b ) is a continuously differentiable function of q

and b, it is nonsingular in an open set that contains q 0 . Since the coefficient matrix of &v& in Eq.
(5.11.36) is nonsingular, the ODE existence theorem of Section 4.7.3 shows that the initial-value
problem of Eqs. (5.11.36) and (5.11.37) has a unique solution v(t, b ) in a neighborhood ( V0 , B0 )
of v = 0 and b that is continuously differentiable with respect to t and b. Further, as shown in
Section 5.2, the solution can be continued on the constraint manifold. These results are
summarized as follows:
Theorem 5.11.1 Well Posed Holonomic ODE: If
(1) all functions that appear in Eqs. (5.11.1) through (5.11.4) are k ³ 1 times
continuously differentiable on a bounded domain D of q-q& -t-b space,
(2) the constraint Jacobian F q (q, b ) has full rank in D,
(3) the mass matrix M( q, b ) is positive definite on the null space of F q ((q, b ) in D,
and (4) q 0 and q& 0 satisfy Eqs. (5.11.1) and (5.11.2),
then Eqs. (5.11.36) and (5.11.37) have a unique solution v(t,b) that is k times
continuously differentiable with respect to t and b in a neighborhood of (t 0 , b ) .
Further, Eqs. (5.11.19), (5.11.24), and (5.11.33) define a unique solution, in a
neighborhood of (q 0 , q& 0 , t 0 , b) , of the variational equations of motion of Section
5.11.2 that is k times continuously differentiable with respect to t and b.
To show that Eqs. (5.11.19), (5.11.24), and (5.11.33) define a solution of the variational
equation of motion, note first that q, q& , and && q satisfy Eq. (5.11.35), for arbitrary dv ,
equivalently Eq. (5.11.4) for all dq such that Φq dq = 0 . Since q, q& , and && q of Eqs. (5.11.19),
(5.11.24), and (5.11.33) satisfy the constraints of Eqs (5.11.1) through (5.11.3), for any
v Î V0 , b Î B0 , v& , and &v& , they satisfy all three forms of constraint. Thus, q, q& , and &&
q is a twice
461
continuously differentiable solution of the variational equations of motion of Section 5.11.2. This
completes the proof.
It is important to note that this result and those that follow are local in nature. That is,
they assure existence, uniqueness, and continuous dependence on data only in a neighborhood of
each point in the domain D of regularity of functions that define the system equations of motion.
The theorem does not guarantee existence, uniqueness, and continuous dependence on data for
all possible time. While local solutions can be extended beyond the neighborhood assured, the
continuations may become infinitesimal and the solution may not exist for all time. No
comprehensive global theory exists for these nonlinear problems.
Algebraic manipulations of Eqs. (5.5.6) through (5.5.11) yield the Index 0 DAE
v + F q (q(v,b ),b T )λ
M(q(v,b ),b )D(q(v,b ),b )&&
- M(q(v,b ),b)U(b )B(q(v,b),b )g (q(v,b ), q& (v, v& ,b ),b ) (5.11.38)
-S(q(v,b ), q& (v, v& ,b ),b ) - Q (q(v,b), q& (v, v& ,b),b, t) = 0
A
for each b Î B0 . Since the algebraic manipulations of Eqs (5.5.6) through (5.5.11) are reversible,
Eq. (5.11.38) with initial conditions of Eq. (5.11.37) has the same solution as the ODE initial-
value problem, hence the same regularity properties. This yields the following result:
Theorem 5.11.2 Well Posed Holonomic Index 0: Under the hypotheses of Theorem
5.11.1, the initial-value problem of Eqs. (5.11.38) and (5.11.37) is well posed.
5.11.6 Well Posed Variational Equations of Motion
A little more work is required to show that the variational equations of motion are well
posed, as follows:
Theorem 5.11.3 Well Posed Holonomic Variational Equations: Under the
hypotheses of Theorem 5.11.1, the variational equations of motion of Eqs. (5.11.1)
through (5.11.5), with initial conditions that satisfy Eqs. (5.11.7) and (5.11.9), are
well posed.
To prove this result, Eqs. (5.11.1) through (5.11.5) and associated initial conditions must
be shown to be equivalent to the ODE initial value problem of Eqs. (5.11.36) and (5.11.37) and
the definitions of Eqs. (5.11.19), (5.11.24), and (5.11.33). The derivation of Section 5.11.4 shows
that a solution of Eqs. (5.11.1) through (5.11.5) defines a solution of Eqs. (5.11.36) and (5.11.37)
. It remains to show that the unique solution of Eqs. (5.11.36) and (5.11.37), with definitions of
Eqs. (5.11.19), (5.11.24), and (5.11.33), define a unique solution of Eqs. (5.11.1) through
(5.11.5). With a unique solution v , v& , and &&v of Eqs. (5.11.36) and (5.11.37), Eqs. (5.11.19),
(5.11.24), and (5.11.33) define unique q, q& , and && q that satisfy Eqs. (5.11.1) through (5.11.3).
Equation (5.11.36) implies Eq. (5.11.35), for arbitrary dv , written in the form
( Ddv ) éëM ( Dv&& - UBγ ) - Q A - S ùû = 0 . Using Eqs. (5.11.33) and (5.11.34), this is
T
dq T ëé Mq
&& - Q A - S ûù = 0 , for all dq such that Φq dq = 0 , which is Eq. (5.11.4).
It remains only to show that the solution q, q& , and &&

q of the variational equations is
unique. For purposes of obtaining a contradiction, assume there is a solution q ¹ q . Then, q is
defined by a solution v of Eqs. (5.11.36) and (5.11.37). Evaluating Eq. (5.11.19) for each
462
solution and subtracting, q - q = V ( v - v ) - U ( h(v ) - h(v ) ) . Since the solution of Eqs. (5.11.36)
and (5.11.37) is unique, v - v = 0 . This implies q = q , which contradicts the assumption. Thus,
the solution of the variational equations of Eqs. (5.11.1) through (5.11.4) is unique, completing
the proof of the theorem.
5.11.7 Well Posed Full DAE of Motion
Applying the Lagrange multiplier theorem of Section 2.2.2 to the variational equations of
motion of Eqs. (5.11.4) and (5.11.5), there exists a unique vector λ Î R nhc such that
dq T ( M(q, b)q
&& - Q A (q, q& , b, t) - S(q, q& , b ) ) + dq TΦ qT ( q, b) λ = 0
for arbitrary dq . This yields the Lagrange multiplier form of the design dependent equations of
motion,
q + Φ Tq (q, b )λ - Q A (q, q& , b, t) - S(q, q& , b ) = 0
M (q , b )&& (5.11.39)
which, with the constraint equations of Eqs. (5.11.1) through (5.11.3); i.e.,
Φ( q , b ) = 0
Φ q (q, b )q& = 0 (5.11.40)
q + γ (q, q& , b) = 0
Φ q (q, b )&&
and initial conditions that satisfy the first two of Eqs. (5.11.40),
q(t 0 ) = q 0 (b )
(5.11.41)
q& (t 0 ) = q& 0 (b )
comprise the Full DAE of motion.
The fact that Eqs. (5.11.39) through (5.11.41) were obtained so easily belies the fact that
they are extraordinarily difficult to solve. While no attempt is made here to solve the Full DAE,
it is shown to be well posed, as follows:
Theorem 5.11.4 Well Posed Holonomic Full DAE: If
(1) all functions that appear in Eqs. (5.11.39) and (5.11.40) are k ³ 1 times
continuously differentiable on a bounded domain D of q-q& -t-b space;
(2) the constraint Jacobian F q (q, b ) has full rank in D;
(3) the mass matrix M( q, b ) is positive definite on the null space of F q (q, b ) in D;
and
(4) q 0 and q& 0 satisfy the first two of Eqs. (5.11.40);
then Eqs. (5.11.39) through (5.11.41) have a unique solution q (t, b ) in a neighborhood
of (q 0 , q& 0 , t 0 , b) that is k times continuously differentiable with respect to t and b; i.e.,
the Full DAE are well posed.
463
This theorem is proved by showing that the DAE initial-value problem of Eqs. (5.11.39)
through (5.11.41) is equivalent to the ODE initial value problem of Eqs. (5.11.36) and (5.11.37)
in v and Eqs. (5.11.19), (5.11.24), and (5.11.33) that define q and its derivatives, which is well
posed. The proof is in two parts, as follows:
Part 1: Solutions of Eqs. (5.11.39) Through (5.11.41) Define Solutions of Eqs.
(5.11.36) and (5.11.37)
Let q , q& , and &&
q satisfy Eqs. (5.11.39) through (5.11.41). Matrices U(b ) and V(b ) of
Eqs. (5.11.10) and (5.11.11) are defined using the given initial value q0 = q(t 0 ) and B(q, b ) of
Eq. (5.11.17) and D(q, b) of Eq. (5.11.23) are defined using the given q . Since the columns of
U(b ) and V(b ) span R ngc , there exist unique v and u such that the given function q is written
as q = q0 + V(b)v - U(b)u . More specifically, multiplying on the left by V T (b ) ,
v = V T (b) ( q - q 0 ) . Since the given q satisfies the first of Eqs. (5.11.40),
Φ ( ( q0 + V (b)v - U(b )u ), b ) = 0 . This is Eq. (5.11.15), which has a unique solution u = h(v, b ) .
Thus, q = q(v, b) = q 0 + V(b )v - U(b)h(v, b) . Similarly, there exist unique w and x such that the
given function q& is written as q& = V(b )w - U(b )x . Multiplying on the left by V T (b ) ,
w = V T (b)q& = v& . Since the given q& satisfies the second of Eqs. (5.11.40),
Φq (q, b)V(b )v& - Φq (q, b )U(b )x = 0 . Using Eq. (5.11.17), x = B(q, b )Φq (q, b)V(b )v& and
q& (v , v& , b) = D(q(v, b ),b )v& . Representing the given q && as && q = V(b )y - U(b )z , y = V T (b )q && = &&
v and
the third of Eqs. (5.11.40) yields z = B(q, b )Φq (q, b )V(b)&& v + B(q, b) γ(q, q& , b ) . Thus,
&&
q (v, v& , &&v, b ) = D(q(v, b ),b) - U(b )B(q(v, b ),b )γ (q (v, b ), q& (v, v& , b ),b ) .
With q, q& , and &&q written as functions of v and its derivatives that satisfy Eqs. (5.11.37),
(5.11.19), (5.11.24), and (5.11.33), it remains only to show that Eq. (5.11.36) is satisfied.
Substituting q, q& , and &&
q as functions of v and its derivatives from Eqs. (5.11.19), (5.11.24), and
(5.11.33) into Eq. (5.11.39) and suppressing arguments,
&& = MUBγ + F qT l - Q A - S = 0
MDv
Multiplying on the left by D T and noting that
D T Φ Tq = ( Φ q D ) = ( Φ q V - Φ q UBΦ q V ) = ( Φ q V - Φ q V ) = 0
T T T
yields
&& - DT MUBγ - DTQA - DTS = 0
DT MDv
which is Eq. (5.11.36). Thus, a solution of the DAE of Eqs. (5.11.39) through (5.11.41) defines
a solution of Eqs. (5.11.36) and (5.11.37).
Part 2: A Unique Solution of Eqs. (5.11.36) and (5.11.37) Defines a Unique
Solution of Eqs. (5.11.39) through (5.11.41)
The functions q( v, b ), q& ( v, v& , b ), and q
&&( v, v& , &&
v, b ) of Eqs. (5.11.19), (5.11.24), and
(5.11.33) are uniquely defined by v and its derivatives. First note that the right side of Eq.
464
(5.11.19) satisfies Eqs. (5.11.15), so q defined by Eq. (5.11.19) satisfies the first of Eqs.
(5.11.40) . Multiplying q& that is defined by Eq. (5.11.24) by Φ q (q, b ) , suppressing arguments
for notational clarity,
Φ qq& = Φq Dv& = Φ q ( I - UBΦq ) Vv& = ( Φq - B -1BΦ q ) Vv& = 0
so q& satisfies the second of Eqs. (5.11.40). Similarly, multiplying &q& of Eq. (5.11.33) by Φ q ,
&& = Φq ( Dv
Φ qq && - UBγ ) = Φq ( I - UBγ ) Vv
&& - Φ q UBγ
= ( Φq - B -1BΦ q ) Vv
&& - B -1Bγ = - γ
&& satisfies the third of Eqs. (5.11.40).

so q
Expanding D of Eq. (5.11.23), Eq. (5.11.36) becomes
&& + V TF qT BT U T ( MUBg + S + Q A )
&& - V TF Tq B T U T MDv
V T MDv
- V T ( MUBg + S + Q A ) (5.11.42)
{
&& + V TF Tq B T U T ( MUBg + S + Q A ) - BT U T MDv
V T MDv && = 0 }
Defining
λ º BT UT ( MUBg + S + QA ) - BT U T MDv
&& (5.11.43)

&& + V T FqT λ - V T ( MUBg + S + QA ) = 0
V T MDv (5.11.44)
Multiplying both sides of Eq. (5.11.43) on the left by U T FqT yields
UT FqT λ = UT FqT BT UT ( MUBg + S + QA ) - UT FqT BT UT MDv

&& (5.11.45)
Taking the transpose of Eq. (5.11.17) yields BT = ( U TF qT ) . Thus, Eq. (5.11.45) reduces to
-1
&& + UT FTq λ - UT ( MUBg + S + QA ) = 0

UT MDv (5.11.46)
Combining Eqs. (5.11.44) and (5.11.46),
[V U ] MDv
T
(
&& + F Tq λ - ( MUBg + S + Q A ) = 0 ) (5.11.47)
Since the columns of V and U span R n , the matrix [ V U ] is nonsingular. Thus,

T
MDv && + F Tq λ - MUBg - S - Q A = 0 or, M ( Dv

&& - UBg ) + F qT λ - S - Q A = 0 . From Eq. (5.11.33),
&& = Dv
q && - UB , yielding
&& + F qT (q, b )λ - S(q, q& , b ) - Q A (q, q& , b, t) = 0
M ( q, b ) q (5.11.48)
Thus, q , q& , q
&&, and λ is a solution of the DAE of Eqs. (5.11.39) through (5.11.41).
465
To see that this solution is unique, first recall that the Lagrange multiplier is unique.
&&, λ) of the DAE such that ( q, q& , q
Next, assume there is a solution ( q, q& , q &&, λ ) ¹ ( q q& , &&
q, λ ) .
The result of Part 1 of the proof shows that there is an associated solution v(t) of the ODE of
Eqs. (5.11.36) and (5.11.37). Since v and v satisfy Eq. (5.11.19) ,
q - q = ( q0 + Vv - Uh (v ) ) - ( q 0 + Vv - Uh(v ) ) . Since the unique solution of Eqs. (5.11.36) and
(5.11.37) is v = v , q - q = 0 . This is a contradiction of the assumption. Thus, the solution of the
DAE is unique. This completes the proof of the theorem.
It is important to note that the proof of the theorem requires that all three forms of the
constraint equations of Eq. (5.11.40) are satisfied. If only one of the three is enforced in methods
that apply ODE integrators to the resulting equations, the results should be regarded with
suspicion.
While tangent space generalized coordinates and the resulting ODE have been used in the
proof of the foregoing theorems, validity of the theorems is not dependent on this specific ODE
formulation.
Existence, uniqueness, and continuous dependence of solutions on problem data of the

equations of mechanical system dynamics (well posed problems) are proved for variational,
tangent space ODE, Index 0 DAE, and Full DAE formulations of the equations of dynamics.
These results put the theory of mechanical system dynamics regarding well posedness on a
footing comparable to that of the theory of ODE.
466
Appendix 5.A Holonomic Tangent Space Code
Code 5.1 Top-Analytical Tangent Space ODE
Code 5.4.1 Planar Double Pendulum (TSODE)
Code 5.4.2a Top Tip Fixed-Spin Stabilized (TSODE)
Code 5.4.2b Top Tip Fixed-Transient (TSODE)
Code 5.4.3 Top Tip on x-y Plane (TSODE)
Code 5.4.4 Spatial Double Pendulum (TSODE)
Code 5.6.1a Top Tip Fixed-Spin Stabilized (TSInd0)
Code 5.6.1b Top Tip Fixed-Transient (TSInd0)
Code 5.6.2 Spatial Double Pendulum (TSInd0)
Code 5.7 Planar Tangent Space Multibody Simulation
Code 5.8 Spatial Tangent Space Multibody Simulation
467
Appendix 5.B Kinematic and Kinetic Derivatives
Derivatives that define constraint Jacobians and operators such as P 2(q, c) that are used
in kinematic velocity and acceleration equations are defined in Sections 3.2 and 3.3. These
quantities and kinetic expressions derived in Sections 4.2 and 4.3 are adequate for explicit
numerical integration using formulations presented in this chapter. Implicit numerical
integration, however, requires an additional derivative of both kinematic and kinetic quantities.
Derivatives of kinematic constraints required for evaluation of P3(q, q& ) and P 4(q, h) in
Sections 5.3 and 5.5 are presented in Section 5.B.1. Derivatives that are required for evaluation
of M 2(q, m ), S q , and S q& in Section 5.3 and 5.5 are presented in Section 5.B.2. Derivatives of
generalized forces are presented in Section 5.B.3.
5.B.1 Derivatives of Kinematic Constraints
5.B.1.1 Derivatives of Planar Constraints
For the distance constraint of Eq. (3.2.2), Eqs. (3.2.3) through (3.2.5) define
P2 (qi j , cij ) of Eq. (3.1.9). From Eqs. (3.2.4), (5.3.30) and (5.3.31), terms required to evaluate
dist
P3dist (qi j , q& ij ) are
(( Φ dist
qi qˆ& i )
qi
)
qˆ& i
qi
= -s¢i T A iT éëf&i2 I 2 2f&i ai + f&i2 PdijT ùû
(( Φ dist
qi qˆ& i )
qi
qˆ& )
i
qj
= f&i2 s¢T AiT ëé I 2 PA js¢j ûù
(
æ dist ˆ&
ç Φ qi qi
è
) qj
ö
qˆ& j ÷ = éë 0 f&i a jT A is¢i ùû
øqi
(5.B.1)
(
æ dist ˆ&
ç Φ qi qi
è
) qj
ö
qˆ& j ÷ = éë0 f& j aiT A js¢j ùû
øq j
(
æ dist ˆ&
ç Φq j q j
è
) qj
ö
qˆ& j ÷ = f& j2s¢T A Tj [ I 2
øqi
PA is¢i ]
(
æ dist ˆ&
ç Φq j q j
è
) qj
ö
qˆ& j ÷ = -s¢jT A Tj éë f& j2 I 2
øq j
2f& j a j - f& j2 PdijT ùû
where
ai = r&i + f& i PA i s¢i
a = r& + f& PA s¢
j j j j j
Thus,
P3dist (qi j ,q& ij ) = éë P 3idist P 3dist
j û
ù (5.B.2)
where
468
P3dist
i = -s¢i T A iT éë f& i2 I 2 2f& i ai + f& i2 Pd i j ùû + 2 éë0 f& i a jT A is¢i ùû + f& 2j s¢jT A Tj [I 2 PAi s¢i ]
P3dist
j = -s¢jT A Tj ëé f& 2j I 2 2f& j a j - f& 2j Pdi j ûù + 2 ëé0 f& j aiT A js¢j ûù + f& i2s¢i T AiT éë I 2 PA js¢j ùû
Terms in P4 (qi j , h) of Eq. (5.3.35), using the subJacobians of Eq. (3.2.4) and the fact
dist
that h is a scalar, are

dist
é I PA is¢i ù
(F dist T
hˆ dist ) = hdist ê T T ú
ë -s¢i A i P s¢i s¢i + s¢i A i di j û
qi qi
T T T
é -I - PA js¢j ù
(F dist T
hˆ dist ) = hdist ê T T ú (5.B.3)
ës¢i A i P -s¢i A i A js¢j û
qi T T
qj
é I PA js¢j ù
(F dist T
qj hˆ dist )
qj
= hdist ê T T T T T ú
ë -s¢j A j P s¢j s¢j - s¢j A j di j û
For the revolute constraint of Eq. (3.2.7), Eqs. (3.2.8) through (3.2.10) define
P2 (qi j , cij ) of Eq. (3.1.9). From Eqs. (3.2.9), (5.3.30), and (5.3.31), terms required to evaluate
rev
P3rev (qi j , q& ij ) are
(( F rev
qi q&ˆ i )
qi
)
q&ˆ i
qi
= éë 0 f&i2 PA is¢i ùû
(( F rev
qi q&ˆ i )
qi
q&ˆ )
i
qj
=0
(
æ rev ˆ
ç F qi q& i
è
) qj
ö
q&ˆ j ÷ = 0
øqi
(5.B.4)
(
æ rev ˆ
ç F qi q& i
è
) qj
ö
q&ˆ j ÷ = 0
øq j
(
æ rev ˆ
ç F q j q& j
è
) qj
ö
q&ˆ j ÷ = 0
øqi
(
æ rev &ˆ
ç Fq j q j
è
) qj
ö
q&ˆ j ÷ = éë 0 -f& j2 PA js¢j ùû
øq j
Thus,
P3rev ( qi j , q& ij ) = éë éë0 f& i2 PA i s¢i ùû éë 0 -f& 2j PA js¢j ùû ùû (5.B.5)
Terms in P4 (qi j , h) of Eq. (5.3.35), using the subJacobians of Eq. (3.2.9) and hrev , are
rev
469
é0 0 ù
(F revT
h
ˆ rev ) =ê T rev ú
ë0 s¢i A i h û
qi qi T
(F revT
qi h
ˆ rev ) qj
=0 (5.B.6)
é0 0 ù
(F revT
qj h
ˆ rev ) qj
=ê T T rev ú
ë 0 -s¢j A j h û
For the translational constraint of Eq. (3.2.13), Eqs. (3.2.3) through (3.2.5) define
P2 tran
(qi j , cij ) in Eq. (3.1.9). From Eqs. (3.2.15), (5.3.30) and (5.3.31), terms required to evaluate
P3tran (qi j , q& ij ) are
(( F tran
qi q&ˆ i ) qi
)
q& i = ê
qi
é -f&i2 v¢i T A iT P f&i2 v¢i Ts¢i - 2f&i v¢i T A iT Pr&i + f&i2 v¢i T A iT di j ù
ëê 0 f&i2 v¢i T A Ti A j v¢j
ú
ûú
(( F tran
qi q&ˆ i ) qi
)
q& i = ê i i i
qj
éf& 2 v¢T A T P -f&i2 v¢i T A Ti A js¢j ù
êë 0
ú
-f&i2 v¢i T A iT A j v¢j úû
é 0 f&i v¢i T A iT Pr&j - ff& & v¢T A T A s¢ ù
è
(
æ tran
ç F qi qˆ& i ) &q j ÷ö = ê
øqi ëê 0 &
-ff & ¢
i j i
i j vi Ai A j v j
T T
¢
i j j
ú
ûú
qj
(5.B.7)
é0 ff & & v ¢ A A s¢ ù
( )
T T
æ tran &ˆ ö
ç F qi q i q& j ÷ = ê &
i j i
&
i j j
ú
è qj øq j ëê 0 ff v
i j i
¢ A
T T
i A ¢ú
jv j û
é 0 f& j2 v¢i T A iT A js¢j ù

(
æ tran ˆ
ç F q j q& j
è
) qj
ö
q& j ÷ = ê
øqi êë 0 f& j v¢i A i A j v¢j úû
2 T T ú
é 0 -f& j2 v¢i T A Ti A js¢j ù

è
(
æ tran ˆ
ç F q j q& j ) qj
&q j ö÷ = ê
øq j êë 0 -f& j v¢i A i A j v¢j úû
2 T T ú
Thus,
P3tran (qi j ,q& ij ) = éë P 3itran P3tran
j û
ù (5.B.8)
where
470
é -f& 2 v ¢T A T P c i ù
P3itran = ê i i i ú
ë 0 di û
éf& 2i v ¢i T A Ti P c j ù
P3tran
j = ê ú
ë 0 djû
( )
c i = f& i2 v ¢i Ts¢i + 2f& i v ¢i T A Ti P (r& j - r&i ) + f& i2 v¢i T A Ti d i j + f& 2j - 2f& i f& j v ¢i A iT A js¢j
( )
c j = - f& 2i - 2f& i f& j + f& 2j v¢i T A Ti A js¢j
( )
d i = f& 2i - 2f& i f& j + f& 2j v¢i T A Ti A j v¢j
( )
d j = - f& 2i - 2f& i f& j + f& 2j v¢i T A Ti A j v¢j
Terms in P4 (qi j , h) of Eq. (5.3.35), using the subJacobians of Eq. (3.2.15) and
tran
T
htran = éë h1tran htran ù
2 û , are
é 0 h1tran A i v¢i ù
(F tran T
h
ˆ tran ) = ê tran T T ú
ëê h1 v¢i A i h1tran v¢i T Ps¢i - h1tran d ijT PA i v¢i + h2tran v¢jT A Tj PA i v¢i ûú
qi qi
é 0 0 ù
( F qtran h
ˆ
T tran
) = ê tran T T ú
êë -h1 v¢i A i - v¢i A i PA j ( h1 s¢j - h2 v¢j )úû
T T tran tran (5.B.9)
i qj
é0 0 ù
(F tran T
qj h
ˆ tran )
qj
=ê ú
êë 0 v¢i Ai PA j ( h1 s¢j + h2 v¢j ) úû
T T tran tran
5.B.1.2 Derivatives of Spatial Constraints

For the distance constraint of Eq. (3.1.3), Eqs. (3.3.2) through (3.3.4) define
P2 (qi j , cij ) of Eq. (3.1.9). From Eqs. (3.3.4), (5.3.30) and (5.3.31), terms required to evaluate
dist
P3dist (qi j , q& ij ) are
471
(( F dist
qi qˆ& i )
qi
)
qˆ& i
qi
= éëp& iT B T (p& i , s¢i ) 2aiT B(p& i , s¢i ) + p& Ti B T (p& i , s¢i )B(pi , s¢i ) ùû
(( F dist
qi qˆ& i )
qi
qˆ& )
i
qj
= ëé -p& iT B T (p& i , s¢i ) -p& Ti B T (p& i , s¢i )B(p j , s¢j ) ûù
(
æ dist ˆ&
ç F qi q i
è
) qj
ö
qˆ& j ÷ = éë 0 - a jT B (p& i , s¢i ) ùû
øqi
(5.B.10)
(
æ dist ˆ&
ç F qi q i
è
) qj
ö
qˆ& j ÷ = éë 0 -aiT B(p& j , s¢j ) ùû
øq j
(
æ dist ˆ
ç F q j q& j
è
) qj
ö
qˆ& j ÷ = ëé -p& Tj B T (p& j , s¢j ) -p& Tj B T (p& j , s¢j )B(pi , s¢i ) ûù
øq i
(
æ dist ˆ
ç F q j q& j
è
) qj
ö
qˆ& j ÷ = éëp& Tj B T (p& j , s¢j ) 2a jT B(p& j , s¢j ) + p& Tj B T (p& j , s¢j )B(p j , s¢j ) ùû
øq j
where
ai = r&i + B(pi , s¢i )p& i
a j = r&j + B(p j , s¢j )p& j
Thus,
P3dist (q i j , q& ij ) = éë P3idist P 3dist
j û
ù (5.B.11)
where
P3dist
i = éëp& Ti B T (p& i , s¢i ) - p& Tj B T (p& j , s¢j ) 2( ai - a j )T B (p& i , s¢i ) + ( p& Ti B T (p& i , s¢i ) - p& Tj B T (p& j , s¢j ) ) B (pi , s¢i ) ùû
P3dist
j = éëp& Tj B T (p& j , s¢j ) - p& iT B T (p& i , s¢i ) 2( a j - ai )T B (p& j , s¢j ) + ( p& Tj B T (p& j , s¢j ) - p& iT B T (p& i , s¢i ) ) B(p j , s¢j ) ùû
Terms in P4 (qi j , h ) of Eq. (5.3.35), using the subJacobians of Eq. (3.3.4), the
dist dist
derivative identity of Eq. (2.6.38), and the fact that hdist is a scalar, are
é I B(pi , s¢i ) ù
(F dist T
η̂dist ) = ηdist ê T ú
ë B (pi , s¢i ) B (pi , s¢i )B(pi , s¢i ) - K (s¢i , d i j ) û
qi qi
T
é I B(p j , s¢j ) ù
(F dist T
η̂dist ) = - ηdist ê T ú (5.B.12)
ë B (p i , s¢i ) B (pi , s¢i )B (p j , s¢j ) û
qi T
qj
é I B (p j , s¢j ) ù
(F dist T
qj η̂dist ) qj
= ηdist ê T T ú
ë B (p j , s¢j ) B (p j , s¢j )B(p j , s¢j ) + K (s¢j , di j ) û
For the spherical constraint of Eq. (3.3.8), Eqs. (3.3.9) and (3.3.10) define P2 (qi j , cij )
dist
of Eq. (3.1.9). From Eqs. (3.3.10), (5.3.30) and (5.3.31), terms required to evaluate P3 (qi j , q& ij )
dist
are all zero, so
472
P3sph (qi j , q& ij ) = 0 (5.B.13)
Terms in P4 (qi j , h ) of Eq. (5.3.35), using the subJacobians of Eq. (3.3.10) and the
sph sph
derivative identity of Eq. (2.6.38) are
(F ) é0 ù
0
sph T
h
ˆ sph =ê sph ú
ë0 -K (s¢i , h ) û
qi
qi
(F sph T
qi h
ˆ sph ) qj
=0 (5.B.14)
é0 0 ù
(F sph T
qj h
ˆ sph ) qj
=ê sph ú
ë0 K (s¢j , h ) û
For the dot1 constraint of Eq. (3.3.13), Eqs. (3.3.15) and (3.3.16) define P2 (qi j , cij ) of
dot1
Eq. (3.1.9). From Eq. (3.3.15), terms required to evaluate P3 (qi j , q& ij ) are
dot1
(( F q&ˆ
dot1
qi i ) qi )
q&ˆ i
qi
=0
(( F q&ˆ
dot1
qi i ) qi
q&ˆ )
i
qj
= éë 0 p& iT B T (p& i , a¢i )B (p j , a¢j ) ùû
(
æ dot1 &ˆ
ç F qi q i
è
) qj
ö
q&ˆ j ÷ = éë0 p& Tj B T (p j , a¢j )B (p& i , a¢i ) ùû
øqi
(5.B.15)
(
æ dot1 &ˆ
ç F qi q i
è
) qj
ö
q&ˆ j ÷ = éë0 p& iT B T (pi , a¢i )B(p& j , a¢j ) ùû
øq j
(
æ dot1 &ˆ
ç Fqj q j
è
) qj
ö
q&ˆ j ÷ = éë 0 p& Tj B T (p& j , a¢j )B(pi , a¢i ) ùû
øqi
(
æ dot1 ˆ
ç F q j q& j
è
) qj
ö
q&ˆ j ÷ = 0
øq j
Thus,
P3dot1 (q i j , q& ij ) = éë P3dot1
i P3dot1
j
ùû (5.B.16)
where
P3dot1
i = éë0 2p& Tj BT (p j , a¢j )B(p& i , a¢i ) + p& Tj BT (p& j , a¢j )B(pi , a¢i ) ùû
P3dot1
j = éë0 2p& Ti BT (pi , a¢i )B(p& j , a¢j ) + p& iT BT (p& i , a¢i )B(p j , a¢j ) ùû
Terms in P4
dot1
(qi j , hdot1 ) of Eq. (5.3.33), using the subJacobians of Eq. (3.3.14), are
473
é0 0 ù
(F dot1T
qi hˆ dot1 ) qi
= ηdot1 ê ú
ë 0 K (a¢i , A ja¢j ) û
é0 0 ù
(F dot1T
qi hˆ dot1 ) qj
= ηdot1 ê T ú
ë 0 B (pi , a¢i )B(p j , a¢j ) û
(5.B.17)
é0 0 ù
(F dot1T
qj hˆ dot1 ) qj
= ηdot1 ê ú
ë 0 K (a¢j , A ia¢i ) û
For the dot2 constraint of Eq. (3.3.18), Eqs. (3.3.20) and (3.3.21) define P2 (qi j , cij ) of
dot1
Eq. (3.1.9). From Eq. (3.3.20), terms required to evaluate P3 (qi j , q& ij ) are
dot1
((F dot2
qi qˆ& i ) qi )
qˆ& i
qi
=0
(( F dot2
qi qˆ& i ) qi
qˆ& )
i
qj
= - éë0 p& iT B T (p& i , s¢i )B(p j , a¢j ) ùû
(
æ dot2 ˆ&
ç F qi q i
è
) qj
ö
qˆ& j ÷ = - éë0 p& Tj B T (p j , a¢j )B(p& i , s¢i ) ùû
øqi
(5.B.18)
è
(
æ dot2 ˆ&
ç F qi qi )
qj
ö
qˆ& j ÷ = - éë0
øq j
( r& + p& B (pi , s¢i ) ) B(p& j , a¢j ) ùû
i
T T
i
T
(
æ dot2 ˆ
ç F q j q& j
è
) qj
ö
qˆ& j ÷ = - éëp& Tj B T (p& j , a¢j ) p& Tj B T (p& j , a¢j )B(p i , s¢i ) ùû
øqi
(
æ dot2 ˆ&
ç Fq j q j
è
)
qj
ö
qˆ& j ÷ = éëp& Tj B T (p& j , a¢j ) d ùû
øq j
where
d = p& Tj B T (p& j , a¢j )B (p j , s¢j ) + p& Tj B T (p& j , s¢j )B (p j , a¢j ) + 2r&jT B (p& j , a¢j )
+ 2p& Tj B T (p j , s¢j )B(p& j , a¢j ) + 2p& Tj B T (p j , a¢j )B(p& j , s¢j )
Thus,
P3dot2 (qi j , q& ij ) = éë P3idot2 P3dot2
j
ùû (5.B.19)
where
P3idot2 = - éëp& Tj B T (p j , a¢j ) 2p& Tj B T (p j , a¢j )B(p& i , s¢i ) + p& Tj B T (p& j , a¢j )B(pi , s¢i ) ùû
P3dot2
j = éëp& Tj B T (p j , a¢j ) -p& iT B T (p& i , s¢i )B (p j , a¢j ) - 2 ( r&iT + p& iT B T (pi , s¢i ) ) B (p& j , a¢j ) + d ùû
Terms in P4 (qi j , h ) of Eq. (5.3.33), using the subJacobians of Eq. (3.3.19), are
dot2 dot2
474
é0 0 ù
(F dot2T
qi hˆ dot2 ) qi
= -ηdot2 ê
ë0 K (s¢i , A ja¢j ) û
ú
é0 B(p j , a¢j ) ù
(F dot1T
qi h
ˆ dot1 )
qj
= -ηdot2 ê
ë0 B (pi , s¢i )B(p j , a¢j ) û
T ú (5.B.20)
é 0 B(p j , a¢j ) ù
(F dot1T
qj h
ˆ dot1 )
qj
= ηdot2 ê T
ëB (p j , a¢j ) e ú
û
where
e = K (s¢j , A ja¢j ) + K (a¢j , di j ) + BT (p j , s¢j )B(p j , a¢j ) + BT (p j , a¢j )B(p j , s¢j )
For the Euler parameter normalization constraint, Eqs. (3.3.47) through (3.3.49) define
P2 (pi , cpi ) . From Eq. (3.3.48), P3pi (pi , p& i ) is
pi
P3pi (p i , p& i ) = (( Φ (p ) q&ˆ ) q&ˆ )

pi
i qi i
qi
i
qi
(
= (p Ti p& i )qi q& i ) qi
= (p& Ti p& i ) qi = 0 (5.B.21)
Finally,
é0ù é0 0 ù
(
P4pi (pi , hpi ) = Φpi (pi )qi hˆ pi ) = hpi ê ú = hpi ê0 I ú (5.B.22)
qi
ëp i û qi ë 4û
5.B.2 Derivatives of Kinetic Quantities

Derivative expressions
(
M2(q, m ) = diag ( M1 (q1 )mˆ 1 )q L( M nb (q nb )mˆ nb )q
1 nb
)
( ) ( )
T
Sq (q, q& ) = é Sq1 (q1 , q& 1 ) L Sq nb (q nb , q& nb ) ù
T T
êë úû (5.B.23)
( ) ( )
T
Sq& (q, q& ) = é Sq& 1 (q1 , q& 1 ) L Sq& nb (q nb , q& nb ) ù
T T
êë úû
involve terms for each planar and spatial body in a system. Derivatives required to evaluate the
system level expressions of Eq. (5.B.23) are presented in the following subsections. For
generality, expressions are given for noncentroidal reference frames. Results for centroidal
reference frames are obtained by setting the vector that locates the centroid to zero; i.e., s¢i C = 0 .
5.B.2.1 Derivatives of Planar Kinetics
For the planar mass matrix of Eq. (4.2.20),
é miI m i PA i s¢i C ù
M i (q i ) = ê ú (5.B.24)
ë m i s¢i A i P J¢i
CT T T
û
T
with m = éëm Tri m fi ùû ,
475
é0 -mfi mi A i s¢i C ù
M 2(qi , m i ) = ( M i (qi )m i )q = ê
ˆ ú (5.B.25)
ëê0 - mi s¢i Ai m ri ûú
i CT T
For the velocity coupling term of Eq. (4.2.20),

é m f& 2 A s¢C ù
S i (q , q& ) = ê i i i i ú (5.B.26)
ë 0 û
and
é 0 mi f&i2 PAi s¢i C ù
Siqi = ê ú
ë0 0 û
(5.B.27)
&
é 0 2mi fi A is¢i ù
C
Siq& i = ê ú
ë0 0 û
5.B.2.2 Derivatives of Spatial Kinetics
For the spatial mass matrix of Eq. (4.3.27),
é miI -2m i A i s%¢i CG i ù
M i (q i ) = ê ú (5.B.28)
ë 2m iG i s%¢i A i 4G Ti J ¢i G i û
T C T
T
where G i = G (p i ) . Defining m i = éëm rTi m Tpi ùû and using derivative identities of Eqs. (2.6.30),
(2.6.44), and (2.6.50),
M 2(qi , m i ) = ( M i (qi )mˆ i )q
i
é0 -2m i B(p i , s%¢i CG im pi ) + 2m i A i s%¢i CG (m pi ) ù

= ê ú
( )
ê0 T 2m i s%¢i C A iT m ri + 4J¢i G i m pi + 2m iG Ti s% i¢CC(pi , m ri ) - 4G Ti J ¢iG (m pi ) ú
ë û
(5.B.29)
For the velocity coupling term of Eq. (4.3.27),
& TG
é 2m i Ei G & G T s¢C ù
S i (q i , q i ) = ê
& i i i i
ú (5.B.30)
& &
ë 8G i J ¢iG ip i
T
û
& = G (p& ) , and using the above cited identities and that of Eq. (2.6.43),
where Ei = E(p i ) , G i i
é0 -4mi E(G & TG & G Ts¢C ) + 4m E G &T& ¢C ù

i i i G i T(s i )
Siqi = ê i i i i
ú
& T J¢G
&
ë0 8G i i i û
(5.B.31)
é0 4mi Ei T(G& G Ts¢C ) - 4m E G &T ¢ ù
i i i G (G i s i )
T C
Siq& i = ê i i i
ú
8T(J ¢iG & p ) - 8G & J ¢G
ë0
T
i i i i i û
476
5.B.3 Derivatives of Generalized Forces
It is not possible to anticipate all forms of force that may act on bodies in multibody
systems, so ad-hoc calculation of generalized forces and their derivatives is often required. Two
forms of force that can be systematically accounted for are due to gravity and translational
spring-damper-actuators (TSDA) of Section 4.5.
5.B.3.1 Derivatives of Planar Generalized Forces
For gravitational force in the negative y direction, Eqs. (4.2.15) and (4.2.16) are
Fg = - ò u y gρdA = - gu y ò ρdA = - gmu y
A A
(5.B.32)
n ¢g = - ò s¢ A (f)P u ygρdA = ò s¢ TρdA ( gA T (f )Pu y ) = mgs¢c T A T (f)Pu y
T T T
A A
where u y is the unit vector in the positive y direction and ρ is mass density. Thus, the
gravitational generalized force on a body with a noncentroidal refence frame is
é -u y ù
Q g = mg ê cT T ú (5.B.33)
ë s¢ A (f )Pu y û
T
For a body with generalized coordinates q = éër T f ùû ,
é0 0 ù
Qgq = mg ê ú
ë 0 s¢ A (f)Pu y û
cT T
(5.B.34)
Qgq& = 0
For the TSDA generalized forces of Eq. (4.5.16),

é d ij ù
Qi = ( f/l ) ê T T ú
ë -s¢i A i Pd ij û
(5.B.35)
é -d ij ù
Q j = ( f/l ) ê T T ú
ës ¢j A j Pd ij û
With f, l, and l& of Eqs. (4.5.1), (4.5.5), and (4.5.7),
477
é di j ù
Q iqi = ê T T
- s ¢ A Pd ú (
(1 / l )(Kl qi + C&l qi ) - (f/l2 )l qi )
ë i i ijû
é [ - I - PAis¢i ] ù
+ (f / l) ê T T ú
ëê ëé s¢i A i P - s¢i s¢i - s¢i A i di j ûù ûú
T T T
é di j ù
Q iq j = ê T T
- s¢ A
ë i i ijû Pd ú (
(1 / l)(Kl q j + Cl& q j ) - (f/l2 )l q j )
é éë I PA js¢j ùû ù
+ (f / l) ê ú
ê - s¢i T A Ti P ëé I PA js¢j ûù ú
ë û
é -d i j ù
Q jqi =ê T T
s¢ A Pd
( &
ú (1/ l)(Kl qi + Cl qi ) - (f/l )l qi
2
)
ë j j ijû
é [ I PAis¢i ] ù
+ (f / l) ê T T ú
ë -s¢j A j P [ I PA i s¢i ] û
é - di j ù
Q jq j =ê T T
s ¢ A Pd
ë j j ijû
( &
ú (1 / l )(Kl q j + Cl q j ) - (f/l )l q j
2
)
é - éë I PA js¢j ùû ù (5.B.36)
+ (f / l) ê T T ú
ê éë s¢j A j P s¢jTs¢j - s¢jT A Tj d i j ùû ú
ë û
where
l qi = - (1 / l )dTij [ I PA is¢i ]
&l = - (1 / l2 )d Tal - (1 / l )a T [ I PA s¢ ] + (1 / l)d T é 0 f& A s¢ ù
qi ij qi i i ij ë i i iû
l q j = (1 / l)d Tij éë I PA js¢j ùû

&l = - (1 / l 2 )d Tal - (1 / l )aT é I PA s¢ ù + (1 / l )dT é 0 f& A s¢ ù
qj ij qj ë j jû ij ë j j jû
a = r& j + f& jPA js¢j - r&i - f& i PA i s¢i
Similarly,
478
é di j ù&
Qiq& i = C(1/ l) ê T T ú l q& i
ë - s¢i A i Pd i j û
é di j ù&
Qiq& j = C(1 / l) ê T T ú l q& j
ë - s¢i A i Pdi j û
(5.B.37)
é - di j ù &
Q jq& i = C(1 / l) ê T T ú l q& i
ë s¢j A j Pdi j û
é - di j ù &
Q jq& j = C(1 / l) ê T T ú l q& j
ë s¢j A j Pd i j û
where
&l = (1/ l)d T [ - I - PA s¢ ]
q& i ij i i
&l & = (1/ l)dT é I PA s¢ ù

qj ij ë j jû
Finally, for the RSDA with generalized forces in Eqs. (4.5.17) and (4.5.19), derivatives
with respect to q i and q j are
é0 0 ù
Q i qi = ê ú
ë0 - K û
é0 0 ù
Qiq j = ê ú
ë0 K û
(5.B.38)
é0 0 ù
Q jqi = ê ú
ë0 K û
é0 0 ù
Q jq j = ê ú
ë0 -K û
and derivatives with respect to q& i and q& j are
é0 0 ù
Qi q& i = ê ú
ë 0 -C û
é0 0 ù
Qi q& j = ê ú
ë0 C û
(5.B.39)
é0 0 ù
Q jq& i = ê ú
ë0 C û
é0 0 ù
Q jq& j = ê ú
ë 0 -C û
479
5.B.3.2 Derivatives of Spatial Generalized Forces
For gravitational force in the negative z direction, Eq. (4.3.14) is
Fg = - ò u z gρdV = - gu z ò ρdV = - gmu z
V V
(5.B.40)
n¢g = - ò s%¢A u z gρdv = - ò s%¢ρdV ( gA Tu z ) = - ò²
T
s¢ρdV ( gA Tu z ) = - mgs%¢ c A Tu z
V v V
where uz is the unit vector in the positive z direction and ρ is mass density. Thus, the
gravitational generalized force on a body with a noncentroidal refence frame in angular velocity
generalized coordinates is
é -u z ù
Q g = mg ê ú (5.B.41)
ë - mgs%¢ A u z û
c T
T
For a body with generalized coordinates q = éë r T pT ùû and a noncentroidal reference
frame, from Eq. (4.3.27),
é Fg ù é -u z ù
Q g = ê T ú = mg ê ú (5.B.42)
ë 2G n¢g û ë -2mgG s%¢ A u z û
T c T
and, from Eqs. (2.6.30) and (2.6.50),

é0 0 ù
Qqg = ê ú
ëê 0 -2mg ( G s¢ C(p, u z ) + T(s¢ A u z ) ) ûú
%
T c
%c T (5.B.43)
Qqg& = 0
For the TSDA generalized forces of Eq. (4.5.10), in terms of generalized coordinates
T
q = éë r T
pT ùû ,
é dij ù
Qi = ( f/l ) ê T ú
ë B (pi , s¢i )dij û
(5.B.44)
é dij ù
Q j = - ( f/l ) ê T ú
ëB (p j , s¢j )dij û
With f, l, and l& of Eqs. (4.5.1), (4.5.5), and (4.5.7),
480
é di j ù
Qiqi = ê T
B ( p , s ¢ )d
( &
ú (1 / l)(Kl qi + Cl qi ) - (f/l )l qi
2
)
ë i i ijû
é [ - I - B(p i , s¢i )] ù
+ (f / l) ê T ú
ëê éë B (p i , s¢i ) B (pi , s¢i )B(pi , s¢i ) + K (s¢i , d i j ) ùû ûú
T
é di j ù
Qiq j = ê T
ë B ( p ,
i i s ¢ ) d ú
ijû
(
(1/ l)(Kl q j + C&l q j ) - (f/l2 )l q j )
é ëé I B(p j , s¢j )ùû ù
+ (f / l) ê ú
ê BT (p i , s¢i ) éë I B(p j , s¢j ) ùû ú
ë û
é di j ù
Q jqi = -ê T
B ( p , s¢ )d ú ((1 / l)(Kl qi + Cl& qi ) - (f/l2 )l qi )
ë j j ijû
é [ - I - B(p i , s¢i )] ù
- (f / l) ê T ú
ë B (p j , s¢j ) [ - I - B(p i , s¢i )] û
é di j ù
Q jq j = -ê T
ë B ( p j , s ¢
j ) d ijû
( &
ú (1/ l)(Kl q j + Cl q j ) - (f/l )l q j
2
)
é ëé I B(p j , s¢j )ûù ù (5.B.45)
- (f / l) ê T ú
ê éë B (p j , s¢j ) BT (p j , s¢j )B(p j , s¢j ) + K (s¢j , d i j ) ùû ú
ë û
where
l qi = -(1 / l )dTij [ I B(p i , s¢i )]
&l = -(1 / l2 )d Tal - (1 / l)a T [ I B(p , s¢ )] + (1/ l)d T [ 0 B(p& , s¢ )]
qi ij qi i i ij i i
l q j = (1/ l)d Tij éë I B(p j , s¢j ) ùû

&l = -(1 / l2 )d Tal + (1 / l)aT é I B(p , s¢ )ù + (1 / l)dT é 0 B(p& , s¢ ) ù
qj ij qj ë j j û ij ë j j û
a = r&j + B(p j , s¢j )p& j - r&i - B(p i , s¢i )p& i

Similarly,
481
é di j ù&
Qiq& i = C(1/ l) ê T ú l q& i
ë B (p i , s¢i )di j û
é di j ù&
Qiq& j = C(1/ l) ê T ú l q& j
ë B (pi , s¢i )di j û
(5.B.46)
é di j ù&
Q jq& i = - C(1/ l) ê T ú l q& i
ë B (p j , s¢j )di j û
é di j ù&
Q jq& j = - C(1/ l) ê T ú l q& j
ë B (p j , s¢j )di j û
where
&l = (1/ l)dT [ - I - B(p , s¢ )]
q& i ij i i
&l & = (1/ l)d T é I B(p , s¢ ) ù

qj ij ë j j û
For the spatial RSDA of Fig. 4.5.3, differentiating c º cosqi j = v¢xiT A iT A j v¢xj and
s º sinqi j = v¢yiT A iT A j v¢xj associated with Eq. (4.5.13) with respect to p j ,
- sθi jp j = v¢xi T A Ti B(p j , v¢xj )

cθ i jp j = v¢yiT A Ti B(p j , v¢xj )
Multiplying the first equation by s, the second by c, subtracting, and using s 2 + c 2 = 1 ,

θ i jp j = cv¢yi T A Ti B(p j , v¢xj ) - sv¢xi T A iT B(p j , v¢xj ) = ( cv¢yi T - sv¢xi T ) A Ti B(p j , v¢xj ) (5.B.47)
Similarly, differentiating with respect to p i ,
-sθi jpi = v¢xjT A Tj B(pi , v¢xi )

cθ i jpi = v¢xjT A Tj B(p i , v¢yi )
and manipulating as above,

θi jpi = cv¢xjT A Tj B(p i , v¢yi ) - sv¢xjT A Tj B(p i , v¢xi ) (5.B.48)
Using the identity G(pi )p j = -G(p j )pi , Eq. (4.5.14) may be written in the form
θ& ij = 2v¢ziT (A Ti A jG(p j )p& j - G(pi )p& i )

= 2v¢ziT ( - A Ti A jG(p& j )p j + G(p& i )pi )
Differentiating the first form with respect to p& i and p& j ,
θ& ijp& i = -2 v¢zi TG(pi )

(5.B.49)
θ& ijp& j = -2 v¢zi T A iT A jG(p j )
482
Differentiating the second form with respect to p i and p j ,
θ& ijpi = -2pTj GT (p& j )A Tj B(pi , v¢zi ) + 2 v¢ziTG(p& i )

(5.B.50)
θ& ijp j = -2 v¢zi T ( A iT A jG(p& j ) + A iT B(p j , G(p& j )p j ) )
Using Eqs. (5.B.47) through (5.B.50) with the torque of Eq. (4.5.11),
tpi = K ( cv¢xjT A Tj B(pi , v¢yi ) - sv¢xjT A Tj B(p i , v¢xi ) )
+ C ( -2p Tj GT ( p& j ) A Tj B(pi , v¢zi ) + 2v¢ziTG(p& i ) )
(
tp j = K ( cv¢yiT - sv¢xi T ) A Ti B(p j , v¢xj ) )
(5.B.51)
(
+ C -2v¢ziT ( A Ti A jG(p& j ) + A iT B(p j , G(p& j )p j ) ) )
tp& i = C ( -2 v¢ziTG(p i ) )
tp& j = C ( -2 v¢ziT A iT A jG(p j ) )
Using Eqs. (2.6.26) and (2.6.50), derivatives of RSDA generalized forces of Eq. (4.5.18)
with respect to p i and p j are
é 0 ù é 0 ù
Qipi = ê i ú
+ê T ú
ë 2tT( v¢z ) û ë 2G (p i )v¢z tpi û
i
é 0 ù
Qip j = ê T
¢ ú
ë 2G (pi )v z tp j û
i
(5.B.52)
é 0 ù é 0 ù
Q jp i =ê i ú+ê ú
ë -2tG (p j )A j B(pi , v¢z ) û ë -2G (p j )A j A i v¢z tpi û
T T T T i
é 0 ù é 0 ù
Q jp j =ê ú + ê -2G (p j )A j A i v¢z tp j úû
ëê -2t ( T( A j A i v¢z ) + G (p j )C(p j , A i v¢z ) ) ûú ë
T i T i T T i
More directly, derivatives of RSDA generalized forces with respect to p& i and p& j are
é 0 ù
Qip& i = ê T ú
ë 2G (pi )v¢z tp& i û
i
é 0 ù
Qip& j = ê T
¢ ú
ë 2G (p i )v z tp& j û
i
(5.B.53)
é 0 ù
Q jp& i =ê ú
ë -2G (p j )A j A i v¢z tp& i û
T T i
é 0 ù
Q jp& j =ê
¢ ú
ë -2G (p j ) A j A i v z tp& j û
T T i
483
Appendix 5.C Key Formulas, Chapter 5
484
485
Appendix 5.D The Trouble with Maggi and Kane Equations
G. A. Maggi published landmark papers at the end of the 19th century (1896,
1901) that establish a system of equations of motion for nonholonomically constrained
mechanical systems. His equations of motion are ordinary differential equations (ODE)
that avoid the need for Lagrange multipliers and the associated differential-algebraic
equations (DAE) of motion.
With the goal of obtaining ODE of dynamics, the Maggi equations were
rediscovered in the 1960s in the setting of nonholonomic systems (Neimark and Fufaev,
1972), for which they were intended. They were again rediscovered or reinvented by
Kane and applied to both nonholonomic and holonomic systems (Kane and Levinson,
1985). Maggi’s equations, equivalently Kane’s equations (Borri, Bottasso, and
Mantegazza, 1990), have been applied to holonomic systems by enforcing constraints
only at the velocity level (Tseng, Ma, and Hulbert, 2003; Bauchau and Laulusa, 2008;
Laulusa and Bauchau, 2008; Garcia de Jalon, Callejo, and Hidalgo, 2012). This raises
both theoretical and practical issues with the resulting ODE. Since configuration
kinematic constraints are not included in the formulation, the numerical solution of the
Maggi/Kane equations leads to “drift” from the holonomic constraints; i.e., error that
accumulates over time.
5.D.1 Kinematics Formulation
For purposes of this analysis, a mechanical system is defined as a collection of
rigid bodies whose position and orientation in an inertial reference frame are defined by
generalized coordinates q = [q1 L q n ] Î R n that must satisfy holonomic constraints
T
of the form
Φ ( q ) = [ F1 ( q ) L F m ( q ) ] = 0 Î R m
T
(5.D.1)
Time independent constraints are treated here, but the formulation can be extended to
time dependent constraints. It is assumed that the vector function Φ (q ) of Eq. (5.D.1)
has two continuous derivatives with respect to its arguments. With q = q (t) ,
differentiating Eq. (5.D.1) with respect to time yields velocity and acceleration
constraints,
Φq (q )q& = 0 (5.D.2)
Φ q (q)&& ( )
q + Φq (q )q&ˆ q& º Φ q (q )&&
q
q + γ (q, q& ) = 0 (5.D.3)
An implementation of the Maggi/Kane equations using tangent space independent

generalized coordinates and enforcing holonomic velocity and acceleration constraints is
presented in Section 5.D.2. It is shown in Section 5.D.3 that solutions of the resulting
ODE need not satisfy Eq. (5.D.1), hence they may not satisfy all equations of system
dynamics.
486
5.D.2 Maggi/Kane Equations with Only Velocity and Acceleration Constraints
At initial time t 0 , with q(t 0 ) = q 0 that satisfies Eq. (5.D.1), define U º ΦTq (q0 )
and V as a solution of Φq (q0 )V = 0 and V T V = I , using singular value decomposition
(Strang, 1980). Note that V T U = 0 and U T V = 0 . Since the columns of U and V span R n
, any vector q& can be uniquely represented as
q& = Vw - Uz (5.D.4)
In order that this q& satisfies Eq. (5.D.2), it is required that
Φq (q )q& = Φq (q )Vw - Φq (q )Uz = 0 (5.D.5)
At t 0 , Φq (q0 )U = U T U is positive definite, since U has full column rank. Hence U T U is

nonsingular. Since Φq (q) is a continuous function of q, the matrix Φq (q )U is
nonsingular in a neighborhood W1 of q 0 ; i.e., in a nonempty open set, possibly large in
extent, that contains q 0 . This justifies the definition
B(q ) º ( Φ q ( q ) U )
-1
(5.D.6)
where B(q ) is a continuously differentiable, nonsingular, matrix function of q in W1 .

Thus, from Eq. (5.D.5), z = B(q)Φq (q )Vw and Eq. (5.D.4) reduces to
q& = Vw - UB(q )Φ q ( q ) Vw = ( I - UB (q )Φ q ( q ) ) Vw = D(q )w (5.D.7)
where
D(q ) º ( I - UB(q)Φq (q ) ) V (5.D.8)
The product
Φq (q )D(q) = Φq (q ) - Φq (q )UB(q)Φq (q ) = Φq (q) - Φq (q ) = 0 (5.D.9)
shows that the columns of the matrix function D(q ) are in the null space of the constraint
Jacobian. Thus, q& of Eq. (5.D.7) satisfies the constraint velocity equation of Eq. (5.D.2),
for arbitrary values of w. The vector w comprises the “kinetic characteristics” in Maggi’s
formulation and “generalized speeds” in Kane’s formulation. Further, the columns of
D(q ) are Kane’s “partial velocities”, which in this formulation provide a differentiable
basis for the null space of the constraint Jacobian.
Dynamics of the system is governed by the variational equation of motion
dq T ëé M(q )&&
q - Q A (q, q& , t) - S(q, q& ) ûù = 0 (5.D.10)
which must hold for all dq that satisfy Φq (q )dq = 0 , where the mass matrix M(q ) is
positive definite on the null space of Φq (q) , QA (q, q& , t) is the vector of generalized
applied forces, and S(q, q& ) is a vector of velocity coupling terms, all assumed to be twice
487
continuously differentiable functions of their arguments. Consistent with d’Alembert’s
principle, no constraint reaction forces or Lagrange multipliers appear in Eq. (5.D.10).
In variational form, Eq. (5.D.7) is the virtual displacement relation
dq = D(q )dw (5.D.11)
Using Eq. (5.D.9),
Φq (q )dq = Φq (q )D(q )dw = 0 (5.D.12)
for arbitrary dw ; i.e., dq of Eq. (5.D.11) satisfies Eq. (5.D.12) for arbitrary dw .
Substituting dq = D(q )dw into Eq. (5.D.10) yields
dw T ëé DT (q )M(q)q
&& - DT (q )Q A (q, q& , t) - D T (q )S(q, q& ) ûù = 0 (5.D.13)
which holds for all dq such that Φq (q )dq = 0 , so by Eq. (5.D.12), it holds for arbitrary
dw . Using this fact and Eq. (5.D.7),
&& - DT (q)QA (q, D(q)w, t) - DT (q)S(q, D(q)w) = 0
DT (q)M(q)q (5.D.14)
which are Maggi’s equations (Neimark and Fufaev, 1972; Borri, Bottasso, and
Mantegazza, 1990). Since Eq. (5.D.14) is comprised of n - m differential equations in
2n + m variables q and w, there is an infinity of solutions. To obtain uniquely solvable
&& must be written in terms of w and its derivatives.
equations, q
Differentiating Eq. (5.D.4) with respect to time,
q = Vw
&& & - Uz& (5.D.15)
&& + γ (q, q& ) = Φq (q )Vw
&& to satisfy Eq. (5.D.3), Φq (q )q
For q & - Φq (q)Uz& + γ(q, q& ) = 0 .
Using Eq. (5.D.6), z& = B(q)Φq (q )Vw
& + B(q )γ(q, q& ) . Substituting this and Eq. (5.D.7)
into Eq. (5.D.15),
q = Vw
&& & - UB(q )Φq (q)Vw
& - UB(q)γ(q, D(q )w )
(5.D.16)
= D(q )w
& - UB(q )γ (q, D(q)w )
From Φ(q )q && + γ (q, q& ) = Φq (q ) ( D(q )w

& - UB(q )γ (q, D(q )w ) ) + γ (q, D(q)w ) = 0 , q
&& of Eq.
(5.D.16) is seen to satisfy Eq. (5.D.3), for arbitrary values of w and w &.
Substituting Eq. (5.D.16) into Eq. (5.D.14) and combining the result with Eq.
(5.D.7) yields
& = DT (q )M(q )UB(q )γ (q, D(q )w )
DT (q )M (q )D(q)w
+ DT (q )Q A (q, D(q )w, t) + D T (q)S(q, D(q )w ) (5.D.17)
q& = D(q )w
which is a system of (2n - m) first order ODE in (2n - m) variables w and q. This is the
form of Maggi/Kane equations of motion that appears in the modern literature (Tseng,
Ma, and Hulbert, 2003; Bauchau and Laulusa, 2008; Laulusa and Bauchau, 2008;
488
Garcia de Jalon, Callejo, and Hidalgo, 2012). As shown in Section 4.6.3, the mass matrix
is positive definite on the null space of the constraint Jacobian, so the matrix
D T (q 0 )M (q 0 )D(q 0 ) = Φq (q 0 ) M (q 0 )ΦqT ( q 0 ) is positive definite. Since DT (q )M(q )D(q )
is a continuous function of q, it is nonsingular in an open set W2 that contains q 0 . From
Eq. (5.D.4), w = V Tq& , so initial conditions are
w(t 0 ) = V Tq& (t 0 )
(5.D.18)
q(t 0 ) = q0
The theory of ODE guarantees that the initial-value problem of Eqs. (5.D.17) and
(5.D.18) has a unique solution ( w(t), q(t) ) on the open set W1 Ç W2 .
5.D.3 Inaccuracy of Maggi and Kane Equations for Holonomic Constraints
Even though the Maggi/Kane ODE initial value problem of Eqs. (5.D.17) and
(5.D.18) has a unique solution, it is not necessarily the solution of the dynamics problem.
The theoretical difficulty in applying the Maggi/Kane equations with holonomic
constraints is that q and w are not Lagrangian generalized coordinates (Pars, 1965); i.e.,
they do not identically satisfy all three forms of the equations of constraint of Eqs.
(5.D.1) through (5.D.3). More specifically, the unique solution of Eqs. (5.D.17) and
(5.D.18) need not satisfy Eq. (5.D.1).
A more important issue is that in numerical solution of the initial value problem
of Eqs. (5.D.17) and (5.D.18), Eq. (5.D.1) is satisfied only to within error tolerances of
the numerical integration method. This is a source of “drift”; i.e., error in satisfying Eq.
(5.D.1) over time. Since the equations of motion are valid only on the regular constraint
manifold C% = {q Î R n : Φ( q ) = 0, rank(Φ q ( q )) = m} , not in a neighborhood of it,
numerical integration error corrupts the equations of motion and leads to an additional
source of error that is difficult to quantify. Since the constraint of Eq. (5.D.1) is an
integral component of system kinematics, the fact that it is not satisfied in numerical
solution of the Maggi/Kane equations is a serious problem. Some implementations of the
Maggi/Kane equations monitor error in satisfying Eq. (5.D.1), stop the simulation when
error exceeds a given threshold, modify the approximate solution to satisfy Eq. (5.D.1),
and continue the simulation, which induces further error in the approximate solution.
A second practical difficulty with the Maggi/Kane equations is that ad-hoc
selection of generalized coordinates may not yield differentiable vector functions that
span the null space of the constraint Jacobian, leading to significant error. The derivation
of Section 5.D.2 avoids this problem, but only in the neighborhood W1 .
489
Appendix 5.E Generalized Coordinate Partitioning
An early approach to solution of the Full DAE of multibody dynamics that shares some
of the characteristics of the tangent space differential geometric method presented in Chapter 5 is
the generalized coordinate partitioning method used for explicit numerical integration of an
independent subset of system generalized coordinates (Wehage and Haug, 1982). The approach
has been adapted to support implicit numerical integration (Haug and Yen, 1992; Negrut, Haug,
and German, 2003), but suffers from complexities that have to date precluded a practical
implementation. Nevertheless, its role in early development of the field of multibody dynamics
warrants a summary presentation.
Since the m ´ n Jacobian Fq (q ) for a system with m holonomic constraint equations
F (q ) = [F1 (q ) L Fm (q )] = 0
T
(5.E.1)
in n generalized coordinates q = [q1 L q n ] is required to have rank m, there exist subvectors

T
u Î R m and v Î R n-m , denoted

q = éëu T v T ùû (5.E.2)
with a reordering of variables, such that in a neighborhood X 0 of q 0 that satisfies Eq. (5.E.1)
Fu (q ) ¹ 0 (5.E.3)
Thus, in a neighborhood V0 of v 0 there exists a unique solution of Eq. (5.E.1)

u = h( v ) (5.E.4)
(
That is, F éëhT ( v ) v T ùû
T
) = 0 , for all v Î V .
0
5.E.1 Gaussian Elimination with the Constraint Jacobian Matrix

To simplify notation in writing the equation for kinematically admissible virtual
displacements δq º x and Fq º éëa i j ùû mń ; i.e., Fqδq = 0 , in component form,
a 11x1 + a12 x 2 + L + a 1n x n = 0
a 21x1 + a 22 x 2 + L + a 2n x n = 0
(5.E.5)
M
a m1x1 + a m2 x 2 + L + a mn x n = 0
or, in matrix form, Ax = 0 . If full pivoting is used and no zero pivots are encountered in forward
elimination (Strang, 1980), after m steps the system reduces to the form
490
é u1 ù
(m) ù ê
M ú
é1 a(m) (m) (m) ( m)
a 13 L a1m a1m +1 L a1n
ê 12 ú ê ú
ê0 1 a (23m ) L a (2m
m)
a (2m
m)
L a (2nm ) ú ê u m ú
ú=0
+1
êM M ê (5.E.6)
M M M ú ê v1 ú
ê ú
êë0 0 0 ( m)
1 a mm+1 L a(m) ú ê M ú
mn û
ê ú
ëv n-m û
where u = [ u1 L u m ] and v = [ v1 L v n -m ] contain reordered elements of x, due to column
T T
pivoting.
Equation (5.E.6) may be written in partitioned form as
Uu + Rv = 0 (5.E.7)
where
(m)
é1 a ( m ) a13 (m ) ù
L a1m
ê 12
ú
0 1 a (23m ) L a (2m
m)
U=ê ú
êM M O M ú
ê ú
ëê0 0 0 1 ûú
(5.E.8)
(m)
é a 1m (m) ù
+1 L a 1n
ê (m) (m)
ú
ê a 2m +1 L a 2n ú
R=ê
M M ú
ê ú
êëa (mm
m)
+1
L a (m) ú
mn û
Note that for any value of v in Eq. (5.E.7), u may be determined through back substitution. Thus,
v may be treated as a vector of independent variables and u as a vector of dependent variables.
The vector of generalized coordinates q is said to be partitioned into independent and dependent
coordinates, according to column pivoting that occurs during factorization of A = Fq (q ) that
leads to Eq. (5.E.6).
The matrix U is obtained by carrying out elementary row operations on Fu , both have the
same rank. Since U = 1 , U has full rank and so does Fu . The foregoing Gaussian elimination
process thus defines the desired partitioning of Eq. (5.E.2).
5.E.2 The Differential-Algebraic Equations of Motion
The Lagrange multiplier form of the equations of motion of Eq. (4.10.13) are
q + FqT (q )l = Q(q, q& ,t)
M(q)&& (5.E.9)
Differentiating Eq. (5.E.1) yields the velocity and acceleration equations

Fq (q )q& = 0 (5.E.10)
491
Fq (q )&& ( )
q = - Fq (q )q&ˆ q& º g (q, q& )
q
(5.E.11)
With initial conditions

q(t 0 ) = q0
(5.E.12)
q& (t 0 ) = q& 0
where F (q0 ) = 0 and Fq (q0 )q& 0 = 0 , Eqs. (5.E.9), (5.E.1), (5.E.10), and (5.E.11) comprise the
Full DAE of motion that is to be solved.
Combining Eqs. (5.E.9) and (5.E.11), in matrix form,
é M(q) F qT (q )ù é q
&& ù éQ(q, q& , t)ù
ê úê ú = ê
0 û ë l û ë g (q, q& ) úû
(5.E.13)
ëF q ( q )
To show that the coefficient matrix on the left of Eq. (5.E.13) is nonsingular; i.e., it uniquely
determines accelerations and Lagrange multipliers, suppressing arguments, define
éM F qT ù éa ù
ê ú =0
0 û êë b úû
(5.E.14)
ëF q
The coefficient matrix is nonsingular if and only if Eq. (5.E.14) implies a and b are zero.
Expanding Eq. (5.E.14),
Ma + F Tqb = 0
(5.E.15)
Fqa = 0
Multiplying the first equation on the left by a T yields
a T Ma + a T F qTb = a T Ma + ( F q a ) = a T Ma = 0
T
(5.E.16)
where the second of Eqs. (5.E.15) was used. Since M is positive on the null space of Fq and a
is in the null space, a = 0 . The first of Eqs. (5.E.15) thus reduces to
F Tq b = 0
Since the kinematic constraints are independent, b = 0 and the coefficient matrix in Eq. (5.E.13)
is nonsingular.
5.E.3 Partitioning the DAE of Dynamics
T
With the reordering of variables q = éëuT v T ùû defined by the foregoing process,
Φu ( q 0 ,t 0 ) ¹ 0 , u is a dependent set of generalized coordinates, and v is an independent set. By
the implicit function theorem of Section 2.2, if Φ ( q ) has k continuous derivatives with respect
to all variables, there is a k-times continuously differentiable function h ( v ) of Eq. (5.E.4) such
that
492
Φ (h ( v ) , v ) = 0 (5.E.17)
for all v in a neighborhood V0 of v 0 .

In terms of this partitioning, the kinematic velocity and acceleration equations are
Φ u u& + Φ v v& = 0 (5.E.18)
v = - éë( Φ u u& )u u& + ( Φ uu& )v v& + ( Φ v v& )u u& + ( Φ v v& )v v& ùû º g ( v, v& , t ) (5.E.19)
&& + Φ v &&
Φ uu
Since Φu is nonsingular, Eq. (5.E.18) is solved for u& to obtain
u& = -Φu-1Φ v v& + Φu-1n (5.E.20)

&& to obtain
and Eq. (5.E.19) is solved for u
&& = -Φu-1Φ v &&v + Φu-1g
u (5.E.21)
Partitioning the first of the equations of motion in Eq. (5.E.13),
é M uu M uv ù é&&
u ù éΦuT ù éQu ù
ê vu +
ú vú ê T ú l = ê vú
M vv û êë&&
(5.E.22)
ëM û ëΦ v û ëQ û
where all functions involved are considered to be functions of only v, v& , and t , as a result of
using Eqs. (5.E.4) and (5.E.20) to eliminate dependence on u and u& . Expanding
this partition yields two equations,
u + Muv&&
Muu&& v + ΦTu l = Qu (5.E.23)
u + M vv&&
M vu && v + ΦTv l = Q v (5.E.24)
Since Φu is nonsingular, so is its transpose, and the notation ( ΦTu ) = ( Φu-1 ) º Φu- T is used.
-1 T
Solving Eq. (5.E.23) for l ,
l = Φu- T éëQu - Muu ( -Φu-1Φ v&&

v + Φu-1g ) - Muv&&
v ùû (5.E.25)
Substituting this result into Eq. (5.E.24),

M vu ( -Φu-1Φ v &&
v + Φ u-1 g ) + M vv &&
v
(5.E.26)
+Φ Tv Φ -u T éëQu - M uu ( -Φu-1Φ v &&
v + Φ u-1 g ) - M uv &&
v ùû = Q v
Collecting terms yields

ˆ ( v )&&
M v=Qˆ ( v, v& , t) (5.E.27)
where
ˆ ( v ) º M vv - M vuF -1F + F TF - T M uuF -1F - F TF - T M uv
M u v v u u v v u
(5.E.28)
ˆ ( v, v& , t) º Q v - M vuF -1g - F TF - T ( Qu - M uu F -1g )
Q u v u u
493
To investigate properties of the matrix M̂ , for any given b ¹ 0 and the
associated a º -Φu-1Φ vb , consider the quadratic form
ˆ b = b T é M vv - M vu Φu-1Φ v + Φ Tv Φ -u T M uu Φ -u1Φ v - Φ Tv Φ -u T M uv ù b
bT M ë û
= b T M vvb + b T M vu a + a T M uu a + a T M uvb
é M uu M uv ù é a ù
= ëéa T
b ùû ê vu
T
ú (5.E.29)
ëM M vv û êë b úû
éa ù
= éëa T b T ùû M ê ú > 0
ëb û
since the vector éë a T b T ùû is nonzero and kinematically admissible; 1.e., Φu a + Φ v b = 0 and M
is positive definite on the space of kinematically admissible velocities. The result for M̂ is even
stronger. Since b is an arbitrary nonzero vector, M̂ is positive definite, hence nonsingular.
Since M̂ in Eq. (5.E.27) is nonsingular,

v=M
&& ˆ -1 ( v ) Q
ˆ ( v, v& ,t ) (5.E.30)
is a classical second order ODE in the independent variable v. With initial conditions
v ( t 0 ) = v0
(5.E.31)
v& ( t 0 ) = v& 0
Eq. (5.E.30) has a unique solution in some neighborhood of t 0 . Since the right side of Eq.
(5.E.30) is k-times continuously differentiable in its arguments, the solution is k- times
continuously differentiable. The solution can be continued in time until the constraint Jacobian
becomes row rank deficient; i.e., until a singular configuration such as lock-up or bifurcation
occurs (a bad design), or until a constraint fails to imply the geometry of a well designed system
(a bad model of a good design).
Since Eq. (5.E.30) is equivalent to the Full DAE, with u = h ( v ) , u& = -Φ-u1Φ v v& , and
u&& = -Φu-1Φ v &&v + Φu-1g , they have the same solutions. To solve the Full DAE, it thus remains only
to solve Eqs. (5.E.30) and (5.E.31). An explicit integrator, such as any of those in Section 4.8,
can be used to construct a solution. If a stiff system is encountered, Eqs. (5.E.30) and (5.E.31)
must be solved using an implicit integrator. The complexity of Eqs. (5.E.28), however, makes
computation of derivatives of M ˆ ( v, v& , t) that are required for implicit integration a
ˆ ( v ) and Q
major impediment. While experimental efforts of limited scope (Haug and Yen, 1992; Negrut,
Haug, and German, 2003) have succeeded, no broadly applicable approach to implicit integration
of these equations has yet been found.
Explicit Numerical Integration of Partitioned Equations of Motion
Rather than carrying out the extensive computations required to evaluate terms in Eq.
(5.E.27) for evaluation on &v& , a practical approach based on sparse matrix methods applied to Eq.
494
(5.E.13) has been used (Wehage and Haug, 1982). The matrix equation of Eq. (5.E.13), repeated
here for clarity,
é M(q) F qT (q )ù é &&
q ù éQ(q, q& , t)ù
ê úê ú = ê
0 û ë l û ë g (q, q& ) úû
(5.E.32)
ëF q ( q )
whose coefficient matrix has been shown to be nonsingular, may be formed and efficiently
T
solved for && q = éëu
q and l using sparse matrix solution techniques. The solution && &&T v T ùû yields
&&
&&
v and &&
u at each time step in the solution process. This is equivalent to forming and solving Eq.
(5.E.27) for &v& . Applying an explicit integrator advances the solution for v and v& on the time
grid. Equations (5.E.4) and (5.E.20) then yield u and u& , hence q and q& on the time grid. The
process is continued until the condition number of Fu exceeds an assigned tolerance, at which
time a new partitioning is defined and integration is continued. This process is presented in detail
in Vol. I of this series (Haug, 1989), as it formed the basis for large scale commercial multibody
dynamic simulation software DADS.
To illustrate a key difference between generalized coordinate partitioning and the tangent
space parameterization approach used in Chapter 5, consider a particle on a unit circle shown in
Fig. 5.E.1. The generalized coordinates q = [q1 q 2 ] are subject to the constraint
T
F(q ) = (1/ 2)(q12 + q 22 - 1) = 0 (5.E.33)
with Jacobian Fq (q ) = q T . The tangent space at q 0 is thus spanned by V = Pq0 and the normal
is U = q0 , as shown on the left of Fig. 5.E.1. Generalized coordinates are partitioned as
q = [u v ] , as shown on the right of Fig. 5.E.1. Perturbations in v with the two
T
parameterizations yield very different changes in v and u, as shown in Fig. 5.E.1. The result is a
more stable and efficient computation with the tangent space parameterization, compared with
that with generalized coordinates. This difference has been verified in numerical computation.
Figure 5.E.1 Tangent Space and Generalized Coordinate Parameterization of Unit Circle
495
CHAPTER 6
Tangent Space ODE for Nonholonomic Systems
6.0 Introduction
A broadly applicable tangent space parameterization of nonholonomic multibody system
kinematics is presented in Section 6.1. As in the holonomic case, a basis for the tangent space
and its orthogonal complement are used to represent system configuration. Due to the appearance
of constraints at the velocity level in nonholonomic systems that have no counterpart at the
configuration level, a separate parameterization of velocity space is required. A basis for the
velocity tangent space and its orthogonal complement is used to represent system velocities and
accelerations. A kinematic differential equation couples the configuration and velocity
parameterizations.
A first order ODE initial-value problem is derived that represents nonholonomic
multibody system dynamics in Section 6.2, using tangent space generalized coordinates. As a
result of the separate parameterizations of position and velocity coordinates, a pair of first order
ODE of motion is obtained. Computational algorithms for integrating the equations of motion
with explicit and implicit first order numerical integration methods are presented. Four examples
are presented in Section 6.3. the first is a relatively simple planar three-wheel transporter, the
second and third a moderately complex planar articulated vehicle and a spatial coin rolling on a
plane. The last rather intricate spatial example is an ellipsoid rolling without slip on a moving
surface. Performance of the method in satisfying all three forms of kinematic constraint, based
on error tolerances embedded in the formulation, is verified for each example.
An Index 0 differential-algebraic equation (DAE) formulation of the equations of motion
for nonholonomic systems is derived in Section 6.4, using tangent space generalized coordinates.
First order DAE are obtained that include Lagrange multipliers. Explicit and implicit numerical
integration algorithms are presented for solution of the Index 0 DAE. Two examples are
presented in Section 6.5 using the Index 0 DAE approach, a planar two chassis articulated
vehicle and a classical spatial problem of a disk that rolls without slip on a plane. Performance of
the Index 0 DAE formulation is shown to be comparable to that of the ODE formulation.
Theorems that show four formulations of nonholonomic multibody dynamics are well
posed are proved in Section 6.6.
MATLAB computer codes that implement the formulations presented are provided in
Appendix 6.A and used to obtain numerical solutions for examples treated in the chapter. A
summary of key formulas appearing in the chapter is presented in Appendix 6.B.
496
6.1 Tangent Space Kinematics for Nonholonomic Systems
A nonholonomic system is defined as one that includes at least one nonholonomic
constraint, or differential constraint, and possibly several holonomic constraints. Technically, if
all differential constraints are integrable, the system is holonomic. From a practical point of
view, however, if it is not known whether differential constraints are integrable, the system must
be treated as nonholonomic. The tangent space formulation introduced in Chapter 5 for
holonomic systems is extended to nonholonomic systems, for which configuration and velocity
spaces are independently parameterized. Conditions that assure accuracy of constraint
satisfaction at configuration, velocity, and acceleration levels are embedded in the formulation.
6.1.1 Nonholonomic System Kinematics
Nonholonomic multibody systems are modeled using a set of ngc generalized coordinates
T
q = éëq1 q 2 L q ngc ùû that are subject to nhc < ngc holonomic constraints,
F ( q, t ) = éëΦ1 ( q, t ) ,Φ2 ( q, t ) ,...,Φ nhc ( q, t )ùû = 0

T
(6.1.1)
which include Euler parameter normalization conditions for spatial bodies, and
0 < ndc < ngc - nhc nonholonomic constraints, or differential constraints,
é E1 (q, t) ù
E(q, t)q& = ê M ú q& = e(q, t) (6.1.2)
ê ú
êë E nd (q, t) úû
Functions that appear in the constraints of Eqs. (6.1.1) and (6.1.2) are assumed to have as
many continuous derivatives as are needed in formulating equations of kinematics and dynamics.
The nhc ´ ngc holonomic constraint Jacobian F q (q, t) and the nd ´ ngc coefficient matrix
E(q , t) are each required to be of full row rank. Furthermore, their rows must be mutually
independent; i.e., the (nhc + ndc) ´ ngc matrix
éF q (q, t) ù
C(q, t) º ê ú (6.1.3)
ë E(q, t) û
must have full rank for (q, t) that satisfy Eqs. (6.1.1) and (6.1.2).
Velocity and acceleration equations associated with Eq. (6.1.1) are
F q (q, t)q& = -F t (q, t) º ν h (q, t)
F q (q, t)&& ( )
q = - F q (q, t)qˆ& q& - 2F tq (q, t)q& - F tt (q, t)
q
(6.1.4)
= - P2(q, q& )q& - 2F tq (q,t)q& - F tt (q, t) º - g h (q, q& ,t)
where, with c Î R ngc ,
P2(q, c ) º ( F q (q, ˆt)cˆ ) (6.1.5)
q
and the acceleration equation associated with Eq. (6.1.2) is
497
E(q, t)&& ( )
q = eq (q, t)q& + e t (q, t) - E(q, t)q&ˆ q& - E t (q, t)q&
q (6.1.6)
= eq (q, t)q& + e t (q, t) - E2(q,q)q
& & - E t (q, t)q& º - g nh (q, q,
& t)
where, with k Î R ngc ,
E2(q, k ) º ( E(q, ˆt)kˆ ) (6.1.7)
q
Combined, the first of Eqs. (6.1.4) and Eq. (6.1.2) comprise system velocity constraints,
é ν (q, t )ù
C(q, t )q& = ê h ú º ν(q, t ) (6.1.8)
ë e(q, t ) û
and the second of Eqs. (6.1.4) and Eq. (6.1.6) comprise system acceleration constraints,
é - g (q,q, & t) ù
&& = ê h
C(q, t )q º - γ(q,q,
& t)
& t )úû
(6.1.9)
ë -γ nh (q, q,
Admissible displacements dq over a time increment dt satisfy Eq. (6.1.8), in differential
form; i.e.,
C(q, t )dq = ν(q, t )dt
As in holonomic systems, variational equations of motion cannot be stated in terms of admissible
displacements that occur over a specified time increment. Kinematically admissible virtual
displacements δq , with time held fixed, are defined to satisfy
C(q, t )δq = 0 (6.1.10)
and are essential in formulation of variational equations of motion.
As in Section 5.2, a configuration tangent space formulation is defined that uses columns
of F (q 0 , t 0 ) and null vectors of F q (q0 , t 0 ) to span the regular configuration space
T
q
C(t) {
% = q : Φ(q, t) = 0 and rank ( Φ (q,t) ) = nhc
q } (6.1.11)
In the nonholonomically constrained case, with differential constraints of Eq. (6.1.2), the
parameterization of configuration space cannot represent system velocities. To represent
velocities, a velocity tangent space is defined that uses columns of CT (q0 , t 0 ) and null vectors of
C(q 0 , t 0 ) to span the regular velocity space
{ %
% q, t) = q& : q Î C(t),
D( }
C(q, t)q& = ν(q, t), and rank ( C(q,t) ) = nhc + ndc (6.1.12)
6.1.2 Configuration Tangent Space Parameterization

Parameterization of the regular configuration space of Eq. (6.1.11) is identical to that
presented in Section 5.2 and is only summarized here. As in Section 5.2, a basis for the regular
configuration tangent space is provided by the columns of matrices V and U that are defined by
498
U = F q ( q0 , t 0 ) T
F q (q 0 , t 0 )V = 0
(6.1.13)
V V=I
T
VT U = 0
That is, any generalized coordinate vector q Î R ngc can be uniquely represented as
q = q0 + Vv - Uu (6.1.14)
The vectors v and u are configuration space generalized coordinates that are equivalent to q in a
neighborhood of q0 . Multiplying Eq. (6.1.14) by V and U,
v = V T (q - q0 )
(6.1.15)
u = - ( UTU ) (q - q )
-1 0
which are the inverse of Eq. (6.1.14). Thus,

v0 = 0
(6.1.16)
u0 = 0
As in Section 5.2, for a given v, u is uniquely determined in a neighborhood V0 of
v = 0 as the solution of
0
F (q0 + Vv - Uu, t) = 0 (6.1.17)

denoted
u = h( v, t) (6.1.18)
and evaluated using Newton-Raphson iteration for u,
Dui = BF (q 0 + Vv - Uui , t)
i=1,2,L until F £ utol (6.1.19)
ui+1 = u i + Dui
where the matrix B is defined as
B(q, t) = ( F q (q, t)U )

-1
(6.1.20)
and evaluated using Newton-Raphson iteration,

DB i = -B i F q (q, t)UBi - B i
i = 1, 2,L until F q (q, t)UB - I £ Btol (6.1.21)
Bi+1 = Bi + DB i
with B0 = ( Φ q (q0 , t 0 )Φ Tq (q 0 , t 0 ) ) . The result is
-1
q = q0 + Vv - Uh( v,t) (6.1.22)

which satisfies the holonomic constraint equation of Eq. (6.1.1), for arbitrary v Î V0 .
Since V and U are constant,
499
q& = Vv& - Uh& (6.1.23)
and using the fact that V T V = I and V T U = 0 ,
v& = V T q& (6.1.24)
6.1.3 Velocity Tangent Space Parameterization
The regular velocity tangent space for the constraint of Eq. (6.1.8) at (q0 , t 0 ) is spanned
by the orthonormal columns of a matrix W that defines the null space of C0 º C(q0 , t 0 ) ,
C0 W = 0
(6.1.25)
WT W = I
which may be computed using singular value decomposition of C0T (Strang, 1980). Expanding
Eq. (6.1.25), using the definition of Eq. (6.1.3), F 0q W = 0 and E0 W = 0 . Thus, the columns of
W are a subset of the space spanned by columns of the matrix V ; i.e., W = VR , where R is an
(ngc - nh) ´ (ngc - nh - nd) matrix such that R T R = I . Thus, E0 VR = 0 and R may be
determined using singular value decomposition of ( E 0 V ) , which is more efficient than singular
T
value decomposition of C0T . To confirm that columns of W are orthogonal, note that
W T W = R T V T VR = R T R = I .
Since the columns of W are orthogonal to the linearly independent columns of
% Ì R ngc of Eq.
C = C(q0 , t 0 )T , the columns of W and C0T span the regular velocity space D
0T
(6.1.12). Defining
X º C0T (6.1.26)
by Eq. (6.1.25),
XT W = 0 (6.1.27)
so every vector q& in R ngc may be uniquely written in the form
q& = q& 0 + Ww - Xx (6.1.28)

where w and x are velocity space generalized coordinates and the minus sign is chosen to
indicate projection onto the regular velocity space of Eq. (6.1.12). Multiplying Eq. (6.1.28) by
W T and X T and using Eqs. (6.1.25) and (6.1.27),
w = WT ( q& - q& 0 )
(6.1.29)
x = - ( X X ) X (q - q )
-1
T
& & 0
Thus,
w0 = 0
(6.1.30)
x0 = 0
500
Since C0 has full rank, C 0C0T º ( H 0 )
-1
is nonsingular. Since C(q, t) is continuous,
C(q, t)C0T = C(q, t) X is nonsingular in a neighborhood of (q0 , t 0 ) . Thus, define
H(q, t) = ( C(q, t) X )
-1
(6.1.31)
in a neighborhood of (q0 , t 0 ) . Since H(q, t) satisfies

C(q, t)XH(q,t) - I = 0 (6.1.32)
it can be evaluated using the iterative process of Eq. (6.1.21); i.e.,
DH i = - H iC(q, t) XH i - H i
i = 1, 2,L until CXT H - I £ Htol (6.1.33)
H = H + DH
i+1 i i
where H(q0 , t 0 ) = ( C(q0 , t 0 )X ) .

-1
Substituting Eq. (6.1.28) into Eq. (6.1.8), Cq& 0 + CWw - CXx = n . Using Eq. (6.1.31),
x = HCWw + HCq& 0 - Hn (6.1.34)
q& = q& 0 + Ww - X ( HCWw + HCq& 0 - Hn )
(6.1.35)
= Kw + ( I - XHC ) q& 0 + XHn
where
K º ( I - XHC ) W (6.1.36)
Differentiating Eq. (6.1.28) with respect to time, since W and X are constant,
&& = Ww
q & - Xx& (6.1.37)
Substituting this into Eq. (6.1.9), CWw & - CXx& = - g . Using Eq. (6.1.31),
x& = HCWw & + Hg (6.1.38)
&& = Ww& - X ( HCWw
q & + Hg ) = Kw
& - XHg (6.1.39)
By construction, q& and q

&& of Eqs. (6.1.35) and (6.1.39) satisfy the system velocity and
acceleration constraints of Eqs. (6.1.8) and (6.1.9), for arbitrary w and w& . As in the case of
holonomically constrained systems, this assures satisfaction of velocity and acceleration
constraints, without problems of constraint drift. The vector w is thus comprised of independent
velocity coordinates.
6.1.4 Kinematic Differential Equation
Equation (6.1.24), taken with Eq. (6.1.35), yields a kinematic differential equation
v& = V Tq& = V T Kw + V T ( I - XHC ) q& 0 + V T XHn (6.1.40)
501
that couples configuration and velocity spaces. Displaying arguments of functions involved
makes clear that this is a first order differential equation in v and w ,
v& = V T K (v, t)w + V T ( I - XH (q(v, t))C(q(v, t),t) ) q& 0 + V T XH(q(v, t))n (q(v, t),t) (6.1.41)
6.1.5 Kinematically Admissible Virtual Displacements

Since columns of W and X span R ngc , any virtual displacement δq can be written as
δq = Wa - Xb (6.1.42)
From Eq. (6.1.10), a kinematically admissible δq must satisfy
Cδq = CWa - CXb = 0 (6.1.43)
Using Eq. (6.1.31), b = HCWa . Thus, kinematically admissible virtual displacements are of the
form
δq = Wa - XHCWa = ( I - XHC ) Wa = Ka (6.1.44)
for arbitrary a .
Tangent space configuration coordinates of Section 5.2 are augmented with tangent space
velocity coordinates, which is required since the space of kinematically admissible velocities is of
reduced dimension, relative to the configuration space. The two spaces are coupled by a
kinematic differential equation. Independent generalized coordinates at the configuration and
velocity levels are defined, guaranteeing that all forms of constraint equations are satisfied.
502
Key Formulas
F ( q, t ) = 0 E(q, t)q& = e(q, t) (6.1.1) (6.1.2)
F q (q, t)q& = ν h (q,t) F q (q, t)q
&& = - g h (q, q& ,t) (6.1.4)
éF q (q, t) ù
q = -g nh (q,q,
E(q, t)&& & t) C(q, t) º ê ú (6.1.6) (6.1.3)
ë E(q, t) û
- P2(q, q& )q& - 2F tq (q, t)q& - F tt (q, t) º - g h (q, q& ,t) (6.1.4)
e q (q, t)q& + e t (q, t) - E2(q,q)q
& & - E t (q, t)q& º - g nh (q,q,
& t) (6.1.6)
P2(q, c ) º ( F q (q, ˆt)cˆ ) E2(q, k ) º ( E(q, ˆt)kˆ ) (6.1.5) (6.1.7)
q q
C(q, t )q& = ν(q, t ) && = - γ(q,q,

C(q, t )q & t) (6.1.8) (6.1.9)
F q (q0 ,t 0 )V = 0 VTV = I U = F Tq (q 0 , t 0 ) (6.1.13)
Dui = BF (q0 + Vv - Uu i , t)
u = h( v, t) (6.1.18) (6.1.19)
ui+1 = ui + Dui
DB i = -B i F q (q, t)UB i - Bi
B(q, t) = ( F q (q, t)U )
-1
(6.1.20) (6.1.21)
Bi+1 = Bi + DBi
q = q0 + Vv - Uh( v,t) (6.1.22)
C0 W = 0 WT W = I XºC 0T
(6.1.25) (6.1.26)
DH = - H iC(q, t) XH i - Hi
i
H(q, t) = ( C(q, t) X )
-1
(6.1.31) (6.1.33)
H = H + DHi+1 i i
q& = Kw + ( I - XHC ) q& 0 + XHn K º ( I - XHC ) W (6.1.35) (6.1.36)

&& = Kw
q & - XHg v& = V T Kw + V T ( I - XHC ) q& 0 + V T XHn (6.1.39) (6.1.40)
503
6.2 Tangent Space ODE for Dynamics of Nonholonomic Systems
The tangent space nonholonomic kinematics formulation of Section 6.1 is next combined
with the variational equations of dynamics of Section 4.6 to create ODE of motion.
6.2.1 ODE of Motion
For systems comprised of particles, planar bodies, and spatial bodies, holonomic
constraints of Eq. (6.1.1) must hold, including Euler parameter normalization conditions for
spatial bodies. In addition, nonholonomic constraints, or differential constraints of Eq. (6.1.2),
and the velocity constraints of Eq. (6.1.4) must hold. Combined, they form the system velocity
constraints of Eq. (6.1.6) and the virtual displacement equation of Eq. (6.1.8). Finally, the system
acceleration constraints of Eq. (6.1.7) must hold.
From Eq. (4.6.16), the variational equation of motion for the system is
δq T ( M (q)&&
q - QA (q, q& , t) - S(q, q& ) ) = 0 (6.2.1)
which must hold for all δq that satisfy Eq. (6.1.8). From Eq. (6.1.42), all virtual displacements
that satisfy Eq. (6.1.8) are of the form
δq = Ka (6.2.2)
for arbitrary a , where K = ( I - XHC ) W . Substituting from Eqs. (6.2.2) and (6.1.39) into Eq.
(6.2.1),
α T K(q, t)T ( M(q )K(q, t)w
& - M(q) XH(q, t)γ(q, q& , t) - QA (q, q& , t) - S(q, q& ) ) = 0
which must hold for arbitrary a . Thus,

& = K(q, t)T ( M(q )XH(q, t)γ(q, q& , t) + QA (q, q& , t) + S(q, q& ) ) (6.2.3)
K(q, t)T M(q )K(q, t)w
Displaying arguments in Eqs. (6.1.22) and (6.1.35),

q = q( v, t) = Vv - Uh( v, t)
(6.2.4)
q& = q& ( w , v,t) = K ( v, t)w + ( I - XH( v, t)C( v,t) ) q& 0 + XH( v, t) ν( v,t)
so Eq. (6.2.3) is a first ODE in w and v , the nonholonomic system kinetic ODE.
Suppressing arguments of functions for simplicity of notation, the first order kinematic
ODE of Eq. (6.1.40) and the first order kinetic ODE of Eq. (6.2.3) comprise the first order ODE
of motion for the nonholonomic system,
v& = V T Kw + V T ( I - XHC ) q& 0 + V T XHn
(6.2.5)
& = K T ( MXHg + Q A + S )
K T MKw
With initial conditions q0 and q& 0 that satisfy Eqs. (6.1.1) and (6.1.8) at t 0 , Eqs. (6.1.16) and
(6.1.30) yield initial conditions for the ODE of Eq. (6.2.5),
v0 = 0
(6.2.6)
w0 = 0
504
To see that the matrix K T MK is nonsingular, note from Eq. (6.1.36) at t 0 that K 0 = W .
Since columns of W span the null space of the system velocity constraint Jacobian,
K 0T M 0 K 0 = W T M 0 W must be positive definite, hence nonsingular. Since all functions involved
are continuous in their arguments, K T MK is positive definite, hence nonsingular, in a
neighborhood of ( v0 , t 0 ) . Thus, the ODE of Eq. (6.2.5) can be written in the classical form

(6.2.7)
& = ( K T MK ) K T ( MXHg + Q A + S )
-1
w
The initial-value problem of Eqs. (6.2.7) and (6.2.6) thus has a unique solution in a
neighborhood of ( v0 , w0 , t 0 ) that depends continuously on problem data (see Section 4.7.3); i.e.,
the initial-value problem is well posed.
To evaluate terms in Eqs. (6.2.5) that are needed for numerical integration, the following
derivatives involving holonomic constraints are required:
P2(q, c) = ( F q (q, ˆt)cˆ )
q
g h (q, q& ,t) = P2(q, q& )q& + 2F tq (q, t)q& + F tt (q, t)

F t = F t (q, t) (6.2.8)
F tt = F tt (q, t)
F tq = F tq (q,t)
Similarly, the following derivatives involving nonholonomic constraints are required:
E2(q, k ) = ( E(q, ˆt)kˆ )
q
g nh (q, q& ,t) = -eq (q, t)q& - e t (q,t) + E2(q, q& )q& + E t (q, t)q&
Et = Et (q, t) (6.2.9)
e q = eq (q, t)
e t = e t (q, t)
With these terms, C, ν, and γ are evaluated in Eqs. (6.1.3) to (6.1.9) and
V, U, W , X, B, H, and K are determined in Eqs. (6.1.13), (6.1.25), (6.1.26), (6.1.20), (6.1.31),
and (6.1.36). Finally, problem dependent terms M (q ) , S (q, q& ) , and QA (q, q& , t) of Eq. (4.6.16)
are evaluated.
6.2.2 Explicit Numerical Integration
Explicit integration of the ODE initial-value problem of Eqs. (6.2.5) and (6.2.6), using
Runge-Kutta methods, is as follows:
(1) Define initial conditions q0 and q& 0 at t 0 that satisfy Eqs. (6.1.1) and (6.1.8). Evaluate
the constraint Jacobian F q (q 0 , t 0 ) and matrices U and V, B, and g . Similarly, evaluate
505
C(q0 , t 0 ) of Eq. (6.1.3) and matrices W, X, and K. Obtain initial conditions on v and w
from Eq. (6.2.6) for integration of Eq. (6.2.5).
(2) At time step t i , solve Eq. (6.2.5) to determine v& i and w
& i . Apply an explicit numerical
integrator to proceed on a time grid with step size h to obtain vi +1 and w i +1 . Use Eqs.
(6.1.22), (6.1.35), and (6.1.39) to evaluate q, q& , and q
&& on the time grid.
(3) Monitor the condition number of the reduced mass matrix K T MK , the norms of v and
w, and the number of iterations required to evaluate u, B, and H. If tolerances are exceeded,
define a new time t 0 and associated q 0 and q& 0 . Repeat calculations in Step (1) to define a
new parameterization and initial conditions v 0 and w 0 . This process follows the trajectory
shown in Fig. 5.2.2, moving smoothly across the constraint set.
6.2.3 Derivatives for Implicit Numerical Integration
To use an implicit numerical integration method in solving Eq. (6.2.5), derivatives of all
terms appearing with respect to v and w are required. For the reader who is satisfied with the
use of explicit numerical integration methods, the somewhat intricate manipulations in the
remainder of this section can be bypassed.
Equation (6.2.5) may be written in residual form as
é R1 ù é v& - V Kw - V ( I - XHC ) q& - V XHn ù

T T 0 T
R=ê ú=ê ú=0 (6.2.10)

ë R 2 û ëê K MKw
T
& - K T ( MXHg + Q A + S ) ú
û
Evaluation of the Jacobian of R with respect to v, w, v& , and w
& is required for implicit integration.
The easy part is
éI 0 ù
R v& , w& = ê ú (6.2.11)
ë 0 K MK û
T
Since all other terms depend on q (v, t) and q& ( w, v, t) , as defined in Eq. (6.2.4), the chain rule of
differentiation is required. From Eqs. (6.1.22) and (6.1.35),
q v = V - Uh v
qw = 0
q& v = (( Kwˆ ) - X ( HCqˆ &ˆ ) - XH (Cqˆ& ) + X ( Hνˆ ) + XHν )q

q
0
q
0
q q q v
(6.2.12)
q& w = K
For the residuals of Eq. (6.2.10),
506
(
R1v = -V T ( Kw
ˆ )q + V T X HCq (
ˆ &ˆ 0 ) q
( )
+ V T XH Cq&ˆ 0
q
- V T X ( Hnˆ )q - V T XHνq q v)
R 1w = - V T K
R2v
æ 2K T M Kw
ç
=ç
( )
&ˆ + K T MKw
q
(ˆ ˆ& - K T (·
q
) (
MXHg + Q A + S ) ) ÷ö÷ q
q
(6.2.13)
(( )
v
ç -K T MXH
ç
è q
)
ˆ gˆ + MX ( Hgˆ ) + MXHg + Q A + S
q q q q
÷
÷
ø
- K T ( MXHg q& + QqA& + Sq& ) q& v
R 2 w = -K T ( MXHg q& + QqA& + Sq& ) q& w
where the fact that M is symmetric has been used.

Since q of Eq. (6.1.22) satisfies Eq. (6.1.1), F (Vv - Uh ( v, t), t) = 0 . Taking the
derivative with respect to v, F q V - F q Uh v = 0 . Using Eq. (6.1.18),
h v = BF q V (6.2.14)
From Eq. (6.1.3),

é( F qaˆ ) ù é P2(q, a) ù
( Caˆ )q = ê q
úºê ú (6.2.15)
ê ( Eaˆ )q ú ë E2(q, a )û
ë û
From Eq. (6.1.31), CXH = I and with a constant vector b, CX Hbˆ ( ) + (CXHb
ˆ ˆ ˆ)
q q
= 0 . Using
Eqs. (6.1.31) and (6.2.15), this yields
é P2(q, XHb) ù
( Hbˆ ) q
ˆ ˆˆ
= -H CXHb ( )q
= -H ê ú
ë E2(q, XHb )û
(6.2.16)
Multiplying Eq. (6.1.36) on the right by an arbitrary vector c and differentiating,
( Kcˆ )q º -X ( HCWc
ˆ ˆ ˆ ) - XH ( CWc
q
ˆ ˆ)
q
(6.2.17)
With the foregoing expressions and P3(q, q& ) of Eq. (5.3.17),
((
P3(q , q& ) º F q ( q, ˆt)qˆ& q&ˆ )q
) = (( P2(q, qˆ& )qˆ& )
q q
(6.2.18)
and Eqs. (5.3.18) for holonomic constraints are

g h = P 2(q, q& )q& + 2F tqq& + F tt
γ hq = P3(q, q& ) + 2( F tq qˆ& ) q + F ttq (6.2.19)
γ hq& = 2P 2(q, q& ) + 2F tq
Define
507
E3(q, q& ) º (( E(q, ˆt)q&ˆ ) qˆ& ) = (E2(q,qˆ& )qˆ& )
q q q
(6.2.20)
where E2(q, k ) is defined in Eq. (6.2.9), and establish the identity
(( E(q, ˆt)q&ˆ ) q& ) = 2 ( E(q, ˆt)q&ˆ ) = 2E2(q, q& )

q q& q
(6.2.21)
This result is valid since, as shown in Section 5.3.3, the expression being differentiated with
respect to q& is symmetric in its q& arguments; i.e., E(q, ˆt)qˆ& 1 q& 2 = E(q,t) ((
ˆ qˆ& 2 q& 1 . Thus, for ) q
) (( ) q
)
g nh of Eq. (6.1.5),
( )
g nh º E(q, ˆt)qˆ& q& + E t (q, t)q& - eq (q, t)q& - e t (q, t)
q
= E2(q,q& )q& + E t (q, t)q& - eq (q, t)q& - e t (q, t)
( ˆ& ˆ&
g nhq = E2(q,q)q ) + ( E (q, t)qˆ& ) - (e (q, t)qˆ& )
q
t
q
q
q
- e tq (q, t)
= E3(q,q& ) + ( E (q, ˆt)qˆ& ) - ( e (q, ˆt)qˆ& ) - e

(6.2.22)
t q tq (q, t)
q q
g nhq& = ( ( E(q,
ˆ ˆt)qˆ& ) q& ) + E (q, t) - e (q, t) t q
q q&
= 2E2(q,q& ) + E t (q, t) - e q (q, t)
Derivatives of ν and g defined in Eqs. (6.1.8) and (6.1.9) are thus available. These relations and
evaluation of the kinetic quantities ( M(q)mˆ )q , Sq , Sq& , QqA , and QqA& presented in Appendix 5.B
provide quantities needed to evaluate terms in Eq. (6.2.13), hence the Jacobian of the residual of
Eq. (6.2.10) that is used in implicit numerical integration.
6.2.4 Implicit Numerical Integration Algorithms
Implicit numerical integration algorithms of Section 4.8 for solution of first order ODE
are applied for solution of Eqs. (6.2.5) and (6.2.6). Since trapezoidal integration formulas are
stated in terms of generalized coordinates, their application is rather simple, as presented in
Section 6.2.4.1. Implicit Runge-Kutta integration formulas, in contrast, are stated in terms of
stage values that are not generalized coordinates. They are applied to the explicit ODE of Eq.
(6.2.7) and expanded for solution of Eq. (6.2.5) in Section 6.2.4.2.
6.2.4.1 Trapezoidal Integration
Implicit trapezoidal formulas for integration of v& and w& of Eq. (4.8.40) are
v n = v n -1 + (h / 2) ( v& n -1 + v& n )
(6.2.23)
w n = w n -1 + (h / 2) ( w & n)
& n -1 + w
These equations are used with Eqs. (6.1.22) and (6.1.35) to evaluate q and q& in Eq. (6.2.10),
508
é R1 ù é v& n - V K ( w n -1 + (h / 2) ( w & n ) ) - V T ( I - XHC)q& 0 - V T XHν ù
& n -1 + w
T
ê úºê ú=0 (6.2.24)

ë R 2 û ëê K T MKw & n - K T ( MXHg + S + Q A ) ú
û
Equations (6.2.23) allow all arguments of functions appearing in Eq. (6.2.24) to be written as
functions of v& n and w
& n . Using the chain rule of differentiation with relations derived in Section
T
6.2.3, the Jacobian of the residual of Eq. (6.2.24) with respect to z n º éë v& Tn & nT ùû is
w
éI 0 ù é R1 R1w ù
J trap = ê + (h / 2) ê v º J1 + (h / 2)J 2
ú R 2w úû
(6.2.25)
ë 0 K MK û ëR 2v
T
where
ì
î
(
R1v = -V T í K (·
w n -1 + (h / 2) ( w
& n -1 + w
q
)
& n ) ) + X HCq
ˆ ˆ& 0( ) q
(
+ XH Cqˆ& 0 ) q
ü
- X ( Hnˆ )q - XHn q ý q v
þ
R1w = 0
ìæ T ·
(
ïèç K MKw
R 2v = í øq
)
& n - K T ( MXHg + S + Q A ) ö÷ + K T M(·
Kw (
& n - XHg ) ) üï q
q
ý v
(
ï + K T M Kw
î n )
ˆ& - K T MX ( Hgˆ ) - K T MXHg - K T (S + Q A )
q q q q
ï
þ
- K T ( MXHg q& + Sq& + QqA& ) q& v
R 2w = -K T ( MXHg q& + Sq& + QqA& ) q& w
(6.2.26)
and q v , q& v , and q& w are given by Eq. (6.2.12).
At t 0 , with h small, the first matrix dominates. As shown in Section 6.2.1, it is

nonsingular, so the Jacobian is nonsingular in a neighborhood of q0 , for h sufficiently small.
Newton-Raphson iteration for solution of Eq. (6.2.24) is
J trap Dz i = - R i
i = 0,1,... until R i < intol (6.2.27)
z i +1 = z i + Dz i
At t 0 , with initial conditions of Eq. (6.2.6) and h = 0, Eq. (6.2.24) is solved to obtain an estimate
T
z 0 º éë v& 0T
n
& 0T
w n û
ù to start the numerical integration process. At subsequent time steps, good
estimates for z 0 are available from prior time steps.
Bounds on the norms of v and w, the number of iterations in evaluating u, B, and H, and
the condition number of the coefficient matrix J trap of Eq. (6.2.27) are used to determine whether
a reparameterization is required. If not, the integration process is continued. If so, the current
time is set as t 0 and the current value of q is used as q 0 to redefine V, U, W, X, B, H, and K .
The process is restarted with initial conditions v 0 = 0 and w 0 = 0 and estimates for their time
T
derivatives v& 0 = V T q& 0 and w && 0 . The estimate z 0 º éë v& 0T w
& 0 = WT q & 0T ùû is used to evaluate the
Jacobian of Eq. (6.2.25) and restart the numerical integration process.
509
6.2.4.2 SDIRK Integration
Runge-Kutta numerical integration methods are presented in Section 4.8 for the solution
of first order ODE of the form
y& = f ( t,y ) (6.2.28)
where y is an n-vector variable. A variety of RK integrators can be written in the form

æ i ö
k i = f ç t n + ci h,y n + h å a ijk j ÷ , i=1,...,s (6.2.29)
è j=1 ø
s
y n +1 = y n + h å bik i (6.2.30)
i=1
i
j=1
s
are constants; åb
i =1
i = 1 ; s is the number of stages in integration; k i are stage variables; and h is
the step-size. If diagonal terms of the matrix a = [a i j ] are a ii = a ¹ 0 , i=1,2,…,s, the method is
called Singly Diagonal, or SDIRK. The stiffly-accurate, L-stable, 5 stage, order 4 SDIRK54
formula for first order ODE is defined in Table 4.8.6.
To integrate the ODE of Eq. (6.2.7) using a RK method, stage variables are
T
k i = éëkviT kw iT ùû and arguments of functions in the stage equations are q( v,t) and q& ( v, w,t)
of Eqs. (6.2.4). Evaluated as functions of stage variables, they are
æ i ö
q i = q ç v n + h å a i jkv j , t n ÷
è j=1 ø
(6.2.31)
æ i i
ö
q& i = q& ç v n + h å a i jkv j , w n + h å a i jkw j , t n ÷
è j=1 j=1 ø
Denoting a general function g (t, v , w ) , evaluated as a function of stage variables,
( æ i i ö
g i = g ç t n + ci h,v n + h å a i jkv j , w n + h å a i jkw j ÷ (6.2.32)
è j=1 j=1 ø
Stage equations for Eqs. (6.2.7), written in residual form, are
( æ( i (( ((ö
R1 º kv i - V T ç K ( w n + h å a i jkw j ) + ( I - XHC)q& 0 + XHν ÷ = 0 (6.2.33)
è j=1 ø
(( ( ( ( -1 ( ( ( ( ( (
R (X,t)2 º kw i - ( K T MK ) K T MXHγ + Q A + S = 0 ( ) (6.2.34)
( ( (
(
Multiplying Eq. (6.2.34) by K iT M i K i yields )
( ( ( ( ( ( (( ( (
(
R 2 º ( K T MK ) kw i - K T MXHγ + Q A + S = 0 ) (6.2.35)
510
Equations (6.2.33) and (6.2.35) comprise the residual form of Runge-Kutta discretization
of Eq. (6.2.24). Using derivatives of residual terms in Eqs. (6.2.12) and (6.2.13) and the chain
rule of differentiation with Eq. (6.2.32), the Jacobian of Eqs. (6.2.33) and (6.2.35) for an SDIRK
T
integrator with a ii = a , in variables z i = éëkv Ti kw Ti ùû , is
( (
éI 0 ù é R1v R1w ù ( (
J RK
=ê ( T ( ( ú + hα ê ( ( ú º J1 + hαJ 2 (6.2.36)
ë0 K MK û ë R 2v R 2w û
i.e.,
é Tæ ( i (( ( ( öù
ê V ç K ( w n + h å a i jkw j ) + ( I - XHC)q& 0 + XHn ÷ ú
J RK Dz i = ê è j=1 øú (6.2.37)
ê ( T ( (( (A ( ú
ëê (
K MXHg + Q + S ) ûú
where
ìæ ( · ( ( üï (
( )
( ö ( (ˆ 0 ( ( (( ((
ï
( ) ( )
i
R1v = - V T íç K ( w n + h å a i jkw j ) ÷ - X HCq ˆ& - XH Cqˆ& 0 - XHCq& 0q + X Hnˆ + XHn q ý q v
ç ÷ q q
ïîè j=1 øq q
ïþ
(
R1w = 0
(
ì K
ïï
R 2v = í
(
( T·
(
( (
MKkw i -
( (( (A (
MXH g - Q - S
q
))
+ K
(T ( ·
M (
(
(
Kkw i - XH
(( ü
g) ï
qï(
)
ý qv
ïî
(T( (¶
( q
)
(T ( ·
( ((
)
ï + K M Kkw - K M( XHg ) - K MXHg q + Qq + Sq ï
q
(T ( ((
(
(A (
ïþ
)
( T ( (( (A ( (
(
- K MXHg q& + Qq& + Sq& q& v )
( ( ( (( ( ( (
(
R 2w = - K T MXHg q& + QqA& + Sq& q& w )
(6.2.38)
(
and derivatives of terms with an over score, · , are evaluated as defined in Eq. (6.2.32).
The Newton-Raphson algorithm of Eq. (6.2.27) is used with the same approach outlined
for the trapezoidal method to obtain initial estimates to start the iterative process and to redefine
the parameterization and restart integration, as required.
6.2.4.3 Implicit Integration Algorithm
Implicit integration of Eqs. (6.2.5) and (6.2.6), using trapezoidal and Runge-Kutta
methods is as follows:
(1) Define initial conditions q0 and q& 0 at t 0 that satisfy Eqs. (6.1.1) and (6.1.8). Evaluate
the constraint Jacobian F q (q 0 , t 0 ) and matrices U and V in Eqs. (6.1.13) and the system
velocity coefficient matrix C(q 0 , t 0 ) and matrices W and X in Eqs. (6.1.25) and (6.1.26).
Evaluate initial conditions v 0 = 0 and w 0 = 0 in Eq. (6.2.6).
511
determine Dzi of Eq. (6.2.27) or (6.2.37). Use Eq. (6.2.23) or (6.2.30) to determine
vn and w n . Use Eqs. (6.1.22), (6.1.35), and (6.1.39) to evaluate qn , q& n , and q
&& n on the time
grid.
(3) Monitor the condition number of the integration Jacobian, the number of Newton-
Raphson iterations required in Step 2, the norms of v and w, and the number of iterations
required to evaluate u, B, and H. If tolerances are exceeded, define a new t 0 and associated
q0 and q& 0 . Repeat calculations in Step (1) to define a new parameterization and initial
conditions v 0 and w 0 .
Beginning with the variational equation of motion for a nonholonomic system and tangent
space parameterizations presented in Section 6.1, a first order ODE initial-value problem is
obtained and shown to have a unique solution. Derivatives that are required for implicit
numerical integration are derived and explicit and implicit numerical integration algorithms
based on methods of Section 4.8 are presented.
Key Formulas
v& = V T Kw + V T ( I - XHC ) q& 0 + V T XHn v0 = 0
(6.2.5) (6.2.6)
& = K T ( MXHg + Q A + S )
K T MKw w0 = 0
512
6.3 Numerical Examples with Nonholonomic Tangent Space ODE
Four examples of varying degrees of complexity are studied in this section, using explicit
numerical integration of the tangent space ODE formulation. A simple planar vehicle example,
two spatial models of intermediate complexity, and a rather intricate spatial example of mixed
holonomic-nonholonomic systems are presented. A still more complex example of a three-wheel
motorcycle may be found in (Haug, 2017a). For illustrations using implicit numerical integration,
the reader is referred to Section 6.5, where examples are treated using the Index 0 formulation.
Extensive use of examples is adopted, because there is not yet a broadly applicable library of
joints and differential constraints for a spectrum of practical systems.
6.3.1 Planar Three Wheel Transporter
The three-wheel transporter shown in Fig. 6.3.1 is used to transport material in a
manufacturing facility. The radius R of the rear wheels and the distance from the floor to point a
are the same, so the bed of the transporter is the x¢-y¢ plane and is horizontal. Clockwise torques
TL and TR are applied to the wheels, relative to the x ¢ axis that is along the rear axle, leading to
forces FL = TL /R and FR = TR /R applied at the respective wheel bearings in the y¢ direction to
control motion of the transporter. Point b at the center of the axle is the origin of the x¢-y¢-z¢ body
f ùû = [ x y f ] . Since the front wheel
T
reference frame. Generalized coordinates are q = éër T
T
is on a trailing arm that rotates freely about a vertical pivot in the chassis, it has no influence on
motion in the x¢-y¢ plane. The rear wheels must roll without slip, so the velocity of point b must
be orthogonal to the axle; i.e., to the x ¢ axis. This leads to the differential constraint
( A(f)u¢x ) r& = [ cosf sinf] r& = [ cosf sinf 0 ] q& º E(q)q& = 0

T
(6.3.1)
Figure 6.3.1 Three-Wheel Transporter

Since there are no holonomic constraints, the configuration tangent space is all of R 3 and
the system has three configuration degrees of freedom. In the formulation of Section 6.2, there is
no U, u, or B and V = I , so v = q. The tangent space of the velocity constraint of Eq. (6.3.1) at
initial configuration q 0 is spanned by orthonormal vectors W i such that E(q0 )Wi = 0 , i=1,2 . A
direct calculation yields orthonormal solutions
513
é -sinf0 0ù
ê ú
W = éë W 1
W 2 ùû = ê cosf0 0ú (6.3.2)
ê 0 1 úû
ë
and, since C(q ) = E(q ) ,
T
X = E(q 0 )T = éë cosf 0 sinf0 0 ùû (6.3.3)
Equation (6.1.31) is thus

H(q) = ( E(q) X ) = 1 / (cosfcosf0 + sinfsinf0 ) º 1 / a(q)
-1
(6.3.4)
and since a(q)=(cosfcosf0 + sinfsinf0 ) ¹ 0 in a neighborhood of q 0 , no iteration is required to

evaluate H(q). Equations (6.1.35) and (6.1.36) thus simplify as
æ 1 ö
q& = Kw + ç I - XE ÷ q& 0 (6.3.5)
è a ø
æ 1 ö
K º ç I - XE ÷ W (6.3.6)
è a ø
Finally, Eq. (6.1.5) is
E(q)q ( )
&& = - E(q, t)qˆ& q& = - x& f& sinf + y& f& cosf º - γ
q
(6.3.7)
With the centroid 1 m forward of the rear axle on the y¢ axis, s¢c = [ 0 1] , and with a
T
rear axle length of 1 m, terms required in noncentroidal equations of motion are

é mI mPA(f)s¢c ù
M = ê cT T2 ú
ë ms¢ A (f)P J¢
T
û
é mf& A(f)s¢ ù
2 c
S=ê ú
ë 0 û
é 0 ù (6.3.8)
ê ú
Q A = ê FL + FR ú
êë R ( FR - FL ) úû
é cosf -sinf ù é 0 -1ù

A (f)= ê ú ; P= ê ú
ë sinf cosf û ë1 0 û
The equations of motion of Eqs. (6.2.5) and initial conditions of Eq. (6.2.6) are thus
514
æ 1 ö
q& = Kw + ç I - XE ÷ q& 0
è a ø
1
K T MKw & = K T Q A + K T MXg + K T S (6.3.9)
a
q( t ) = q 0
0
w( t 0 ) = 0
Since there is no u or B to be iteratively determined and H is given by the expression of Eq.
(6.3.4), this is an explicitly defined ODE initial-value problem.
The mass and polar moment of inertia of the transporter, relative to the x¢-y¢ frame, are
m = 10 kg and J ¢ =10 kg × m2 , g = 9.8 m/sec 2 , and R = 1 m. The initial configuration is with

point b on the z axis and the x¢-y¢ and x-y frames aligned, so q 0 = [0 0 0 ] . The initial
T
velocity is 5 m/sec along the y axis, so q& 0 = [ 0 5 0] . Applied forces FL = 20 N , 0 £ t £ 2;

T
FL = -20 N , 2 £ t £ 4; FL = 0, t>4 ; FR = -20 N , 0 £ t £ 2; FR = 20 N , 2 £ t £ 4; FR = 0, t>4 in

the y¢ direction on the left and right rear wheel spindles yield a clockwise torque about the z-
axis for two seconds, followed by a counterclockwise torque about the z-axis for two seconds,
and no applied torque after four seconds.
The x-y trajectory of point b for a 10 sec simulation with the explicit RK4 integrator of
Section 4.8.1.1 is shown in Fig. 6.3.2. Simulations are carried out with MATLAB Code 6.3.1 of
Appendix 6.A. Velocity and acceleration constraint errors are less than 10-15 . Even though a
bound of unity on the norm of w was set as a criterion for reparameterization, it was never
reached, so only one parameterization was used for the entire simulation. The condition number
of the reduced mass matrix was almost exactly one, throughout the simulation.
Figure 6.3.2 ry-rx Trajectory of Three Wheel Transporter
515
6.3.2 Planar Articulated Vehicle
A two chassis articulated vehicle is modeled in the x-y plane, as shown in Fig. 6.3.3. The
front axle of chassis one is steerable, with angle q(t) . Since roll without slip conditions for both
front wheels would be redundant, a single wheel model is placed at the center of the front axle at
point P1 and the roll without slip condition is applied to this wheel model. The chassis are
coupled by a revolute joint at point P3 that is orthogonal to their common plane of motion, 3 m
from the origins of body-fixed reference frames.
Figure 6.3.3 Planar Articulated Vehicle

Six generalized coordinates for the system are
f2 ûù = [ x1 y1 f1 x 2 f2 ]
T
q = ëér1T f1 r2T
T
y2
The revolute joint between the chassis is characterized by the holonomic constraint
Φ(q) = ( r1 - 3A(f1 ) i1¢ ) - ( r2 + 3A(f2 ) i ¢2 ) = 0 (6.3.10)
whose Jacobian is
Φq (q ) = [ I2 -3PA(f1 )i1¢ -I2 -3PA(f2 )i ¢2 ] (6.3.11)
where A(fi ) is the rotation transformation matrix from the x i¢ -yi¢ frame to the x-y frame and i i¢
are unit vectors along the x i¢ axes.
Vectors from origins of body-fixed reference frames to centers of axles are
r = r1 + 3A ( f1 ) i1¢ , r1P3 = r1 - 3A ( f1 ) i1¢ , and r2P2 = r2 - 3A ( f2 ) i¢2 . Velocities of these points are
1
P1
516
r&1P1 = r&1 + 3f& 1PA ( f1 ) i1¢
r& P3 = r& - 3f& PA ( f ) i¢
1 1 1 1 1 (6.3.12)
r&2P2 = r&2 - 3f& 2 PA ( f2 ) i¢2
where P = A( p / 2) and A(f)f = PA(f) .
The velocities of Eqs. (6.3.12) must be orthogonal to unit vectors u1 , u3 , and u 2 ,

respectively, as conditions that the wheels do not slip laterally; i.e.,
1 1 ( 1 1 1 )
u Tr& P1 = [ - sin q cos q] A ( f ) r& + 3f& Pi¢ = 0 , u T r& P3 = j¢T A ( f ) r& - 3f& = 0 , and
T T
3 1 1 1 1 1
u 2 T r&2P2 = j¢2 T A ( f2 ) r&2 - 3f& 2 = 0 , where A T PA = P has been used. Differential constraints that
T
act on the system are thus

é[ - sin( q) cos(q)] A(f1 )T 3 [ - sin( q) cos(q) ] j1¢ 0 0ù
ê ú
E(q, t)q& = ê j1¢T A(f1 )T -3 0 0 ú q& = 0
ê 0 0 j¢2 A (f2 )T
T
-3úû
ë
(6.3.13)
so e(q, t) = 0 . The composite constraint coefficient matrix of Eqs. (6.3.11) and (6.3.13) is
é Φ (q ) ù
C(q, t) = ê q ú (6.3.14)
ë E(q, t) û
As defined in Section 6.1, and in turn in Section 6.2, derivative terms needed for explicit
T
numerical integration of the equations of motion, with χ = éë χ rT1 χ f1 χ Tr2 χ f2 ùû , are
F t = - ν = 0, F tq = F ttq = 0, and
(
P2( q, χ ) = Φ q ( q, tˆ ) χˆ ) q
= éë 0 3χ f1 A( f1 )i1¢ 0 3χ f2 A(f2 )i¢2 ùû
& é[cos q sinq]A(f1 )T 3[cos q sinq]j1¢ 0 0 ù

Et = - q ê ú
ë 0 2´6 û
é 0 [cos q sinq]A(f1 )T Pr&1 0 0 ù
( E t (q, ˆt)q&ˆ ) q
&
= qê
ë 02´6
ú
û
é 0 -[ - sin( q) cos(q)]A(f1 )T Pχ r1 0 ù 0
ê ú
(
E2(q, χ ) = E(q, tˆ )χˆ ) q
= ê0 - j1¢T A(f1 )T Pχ r1 0 0 ú
ê0 0 0 - j¢2 A(f2 ) Pχ r2 úû
T T
ë
With these results, g of Eqs. (6.2.19) may be evaluated. In the planar equations of motion for
this system, M = mI 6´6 , S = 0, and Q A = 0 .
517
With two holonomic and three differential constraints on six generalized coordinates and
velocities, this system has four kinematic degrees of freedom and one kinetic degree of freedom.
Accordingly, there are four first order kinematic equations for v and one first order kinetic
equation for w in Eqs. (6.2.5).
As a numerical example, each of the two chassis has a mass of 1,500 kg and the same
polar moment of inertia. Acceleration due to gravity is 9.8 m/ sec2 . The x ¢ -axes of the chassis
are initially coincident with the global x-axis, with the centroid of the rear chassis at the origin of
the x-y frame, so q 0 = [ 6,0, 0, 0, 0, 0] . The initial velocity of both chassis, consistent with
T
kinematic and differential constraints, is 15 m/sec (35 mph) along the global x axis, so
q& 0 = [15, 0, 0,15, 0, 0] . A steer angle θ = 0.017sin ( t ) rad , for 0 £ t £ 2π, and θ = 0 thereafter is
T
imposed to simulate a lane change maneuver to the left. Simulations are carried out with Code
6.5.1 of Appendix 6.A that supports both the tangent space ODE and Index 0 DAE formulations,
using the explicit RK4 integration algorithm. Results shown in Fig. 6.3.4 for tf = 10 sec, h =
0.001 sec, utol = Btol = Htol = 10-10 , and intol = 0.0001 confirm the lane change trajectory for
the first chassis in the left plot and the orientation of the second chassis relative to the first in the
right plot, subsequently trailing with both chassis aligned. For the 10,000 time steps of the
simulation, the algorithm required 22 reparameterizations (455 time steps per
reparameterization), all due to a limit of 10 imposed on the norm of v. Kinetic energy for all
three simulations is constant to five significant figures for this conservative system.
Figure 6.3.4 Lane Change Maneuver

As a check on constraint error control, Tol = utol = Btol = Htol in Eqs. (6.1.19), (6.1.21),
and (6.1.33) is varied. Results presented in Table 6.3.1 show that maximum norms of position,
velocity, and acceleration constraint errors over the simulation interval are driven toward zero to
computer precision as tolerances are tightened, with little increase in compute time.
518
Table 6.3.1 Maximum Constraint Error for Planar Articulated Vehicle
e-6 7e-13 e-10 e-10
e-9 5e-14 e-10 e-10
e-12 5e-14 6e-15 e-14
6.3.3 Spatial Disk Rolling Without Slip on x-y Plane

The disk with unit radius shown in Fig. 6.3.5 rolls without slip on the x-y plane. The
plane of the disk is defined by body fixed y¢-z¢ axes, where the body fixed x ¢ axis is normal to
the plane of the disk. Unit vector a¢ in the x¢-y¢-z¢ frame from the center of the disk to contact
point C on the periphery with the x-y plane is
é0ù
é0 ù é0 ù
a I
and the vector a = [ a1 a 2 ]

T
satisfies the condition
( a a - 1) / 2 = 0
T
(6.3.16)
The normal to the disk periphery at contact point C in the plane of the disk is the vector a¢ , so
é 0 ù
é 0 ù é0ù
b¢ = êê -a 2 úú = ê ú = ê ú a º b¢a a (6.3.17)
Pa P
Figure 6.3.5 Disk Rolling Without Slip on the x-y Plane
519
Euler parameters are used as orientation generalized coordinates. Contact conditions are
that point C on the disk is in the x-y plane; i.e., u Tz ( r + A(p)a¢aa ) = 0 , and that the vector
b = A (p )b¢ is in the x-y plane; i.e., u Tz A(p)b¢aa = 0 . Combining these conditions, Eq. (6.3.16),
and the Euler parameter normalization condition yield the holonomic constraints
éu Tz ( r + A(p)a¢aa ) ù
ê ú
ê u Tz A(p)b¢aa ú
F(q) = =0 (6.3.18)
ê (p Tp - 1) / 2 ú
ê ú
ë (a a - 1) / 2 û
T
T
where generalized coordinates are q = éër T pT a T ùû Î R 9 . Since F(q) does not depend
explicitly on t, F t = - ν h = 0, F tt = 0, and Fqt = 0 . The holonomic constraint Jacobian is
éuTz u Tz B(p,a¢aa) u Tz A(p)a¢a ù

ê ú
0 u Tz B(p, b¢aa) u Tz A(p)b¢a ú
Fq = ê (6.3.19)
ê0 pT 0 ú
ê ú
ë0 0 aT û
v C = r& + A(p)ω &
% ¢a¢aa = r& + A(p)A T (p)A(p)a ¢a a
(6.3.20)
&
= r& + A(p)a¢ a = r& + ( A(p)a¢ aˆ ) p& = r& + B(p,a¢ a)p&
a a p a
components of v C are zero; i.e. u Tx v c = 0 and u Ty v c = 0 . With Eq. (6.3.20), this is
éu T u Tx B(p,a¢a a) 0 ù
E(q)q& º ê Tx ú q& º e(q, t) = 0 (6.3.21)
êëu y u Ty B(p,a¢a a) 0 úû
Since E does not depend explicitly on t and e = 0 ; Et = 0, eq = 0, and e t = 0 . With four

holonomic and two differential constraints on nine generalized coordinates, the system has five
kinematic degrees of freedom and three kinetic degrees of freedom.
Kinetic and force terms in the equations of motion are
520
é mI 0 0ù
M(q) = ê 0 4G (p)J¢G(p) 0 úú
ê T
êë 0 0 0 úû
é 0 ù
ê
& = 8G (q)J
S(q,q) T
& ú
& ¢G(q)p (6.3.22)
ê ú
êë 0 úû
é - mgu z ù
Q = êê 0 úú
A
êë 0 úû
Derivatives of terms that are required for explicit integration of the equations of motion,
T
with χ = éë χ Tr χ pT χ aT ùû , are
é 0 u Tz B(χ p , a¢aa) + u Tz B(p, a¢a χ a ) u Tz M (p, χ p )a¢a ù

ê ú
0 u Tz B(χ p , b¢aa) + u Tz B(p, b¢a χ a ) uTz M(p, χ p )b¢a ú
P 2(q, χ ) = ( Fq (q)χˆ )q = ê
ê0 χ pT 0 ú
ê ú
êë 0 0 χ aT úû
é 0 u Tx B( χ p , a¢a a) u Tx M (p, χ p )a¢a ù
E2(q , χ ) = ( E(q) χ )q = ê
ˆ ú
êë 0 u y B( χ p , a¢a a) u y M (p, χ p )a¢a úû
T T
For a disk with thickness 0.01 m, radius 1 m, mass m = 10 kg, and g = 9.6 m/sec 2 , the
density is 1000 / p kg/ m3 , so J x¢x¢ = m / 2 = 5 and J y¢y¢ = J z¢z¢ = m(3 + 10-4 ) / 12 = 2.5001 . The disk
is initially in the y-z plane, with x ¢ and x and y¢ and y axes parallel, so the initial configuration
is r 0 = u z , p 0 = [1 0 0 0] , and a 0 = [0 -1] . The disk initially rolls along the y axis, with a
T T
small rotation perturbation about the y axis, so the initial angular velocity-Euler parameter
derivative relation is [ - omegax0 0.01 0] = 2G(p 0 )p& 0 . Appending this relation to Eq. (6.1.6)
T
yields 9 equations in 9 unknowns that are solved for q& 0 .

The formulation is implemented in MATLAB Code 6.5.2 of Appendix 6.A that supports
both the tangent space ODE and Index 0 DAE formulations, using the explicit RK4 integration
method. Plots of centroid ry-rx trajectory and rz-coordinate vs time for initial condition omegax0
= -1 rad/sec are shown in Fig. 6.3.6. The nearly straight segments of the ry-rx trajectory on the
left are with r z » 1 and define the direction of roll, which changes after each loop and associated
dip of the centroid shown in the right plot of Fig. 6.3.6. Numerical integration is carried out
using utol = Btol = Htol = intol = 10-7 and h = 0.001 sec with the RK4 algorithm. In 40,000 time
steps, 127 reparameterizations were required (315 time steps per reparameterization), all due to a
limit of 0.7 on the norm of v. Kinetic energy in each simulation is constant to seven significant
figures for this conservative system.
521
Figure 6.3.6 Disk Centroid rx-ry Trajectory and z-Coordinate vs Time
As a check on constraint error control, Tol = utol = Btol = Htol in Eqs. (6.1.19), (6.1.21),
and (6.1.33) is varied in simulations. Data in Table 6.3.2 show that maximum norms of position,
velocity, and acceleration constraint norm errors over the simulation interval are driven toward
zero, to computer precision, as the tolerances are tightened. There is little increase in compute
time associated with the tighter tolerances.
Table 6.3.2 Maximum Constraint Error for Rolling Disk
e-6 2e-8 3e-7 e-6
e-9 2e-11 2e-9 2e-8
e-12 3e-14 2e-13 e-12
6.3.4 Ellipsoid Rolling Without Slip on a Moving Surface

A more complex example is the ellipsoid of Fig. 6.3.7 that rolls without slip on a moving
surface, whose geometry is defined by the equation
z = d(x1 ,y1 ) + g(t) º ε(x12 + 2y12 ) + amp (1 - cos(ωt) ) (6.3.23)
where variables x1 and y1 are coordinates in the x-y plane that locate points on the moving
surface. They are not to be confused with the x and y coordinates of the centroid of the ellipsoid.
With e and amp small, the surface deviates only moderately from the x-y plane.
522
Figure 6.3.7 Ellipsoid Rolling on Moving Surface
The velocity of point Pe on the ellipsoid that is in contact with the surface, at point Ps in
the surface, is zero relative to the tangent plane of the surface; i.e., it cannot slip on the surface.
T
In terms of body fixed coordinates a¢ = éë a ¢x a ¢y a ¢z ùû in a centroidal x¢-y¢-z¢ body fixed frame
in the ellipsoid, the equation that defines the boundary of the ellipsoid is
Y (a¢) =
1
2
( 2 2
)
( a ¢x / a ) + ( a ¢y / b ) + ( a ¢z / d ) - 1 = ( a¢T La¢ - 1) = 0
2 1
2
(6.3.24)
where L = diag (1 / a 2 1 / b2 1 / d 2 ) .
The vector that locates point Pe on the boundary of the ellipsoid that is in contact with the
surface at time t is
r Pe = r + A(p)a¢ (6.3.25)
T
where p = éë e 0 e T ùû is the vector of Euler parameters that defines orientation of the x ¢-y¢-z¢
frame relative to the x-y-z frame, and A(p) is the associated orientation transformation matrix.
The vector that locates point Ps on the surface that is in contact with the ellipsoid at time t is
é x1 ù
s =ê
Ps ê y1 ú (6.3.26)
ú
êë d(x1 , y1 )+g(t) úû
The condition that points Pe and Ps coincide at time t is the time dependent holonomic constraint
r + A(p)a¢ - s Ps = 0 (6.3.27)
It is important to note that points Pe and Ps are not fixed in the ellipsoid or surface,
respectively. Vectors r Pe and s Ps locate points on trajectories of contact, not points fixed in their
respective surfaces. To emphasize this point, note that if g(t) is constant, the surface on which the
523
ellipsoid rolls is fixed in space and the velocity of each point on the surface is zero. The time
T
derivative of vector s Ps in Eq. (6.3.26), namely s& Ps = éë x& 1 y& 1 d x1 x& 1 + d y1 y& 1 ùû , is the time
derivative of a point on the trajectory of contact, not the velocity of any point fixed in the
surface, and likewise for the trajectory of contact points on the ellipse.
The normal to the ellipsoid at point Pe , represented in the x¢-y¢-z¢ frame, is the transpose
of the gradient of the function in Eq. (6.3.24),
T
n¢e = Y aT¢ = ëéa ¢x / a 2 a ¢y / b 2 a ¢z / d 2 ûù º La¢ (6.3.28)
and the normal to the surface is the transpose of the gradient of z - d(x1 , y1 ) - g(t) = 0 from Eq.
(6.3.23),
T
n s = éë - d x1 - d y1 1ùû (6.3.29)
The vector A(p)n¢e must be collinear with ns , which is enforced by the condition that A(p)n¢e is
orthogonal to a basis for the tangent plane to the surface at the point of contact. By inspection, a
nonzero linearly independent pair of vectors that are orthogonal to ns is
T
w1 = éë1 0 d x1 ùû
T
(6.3.30)
w 2 = éë 0 1 d y1 ùû
and the required orthogonality conditions are

w1T A(p)La¢ = 0
(6.3.31)
w T2 A(p)La¢ = 0
Generalized coordinates for this system are
T
q = ëér T pT a¢T x1 y1 ùû Î R12
Holonomic kinematic constraints on q, from Eqs. (6.3.27), (6.3.31), (6.3.24), and the Euler
parameter normalization condition, are
ér + A (p)a¢ - s P ù
ê T ú
ê w1 A(p)La¢ ú
ê w T A(p)La¢ ú
F(q) = êê ú
2
ú = 0Î R
7
(6.3.32)
1 T
ê
2
( p p - 1) ú
ê ú
ê 1 a¢T La¢ - 1 ú
êë 2 ( ) úû
The Jacobian of these constraints is
524
éI B(p, a¢) A (p ) -s Px1 -s yP1 ù
ê 3 ú
ê0 w1T B (p, La¢) w1T A ( p ) L a¢T LA T ( p ) w1x1 a¢T LA T ( p ) w1y1 ú
Fq = ê 0 w T2 B (p, La¢) w T2 A ( p ) L a¢T LA T ( p ) w 2x ú
a¢T LA T ( p ) w 2y1 ú (6.3.33)
ê 1
ê0 pT 0 0 0 ú
ê0 ú
êë 0 a¢ L
T
0 0 ûú
For the paraboloid d(x1 , y1 ) = ε(x12 + 2y12 ) , from Eqs. (6.3.26) and (6.3.30),
s Px1 = [1 0 2εx1 ]
T
s Py1 = [ 0 1 4εy1 ]
T
(6.3.34)
w1x1 = 2εu z , w1y1 = 0, w 2x1 = 0, w 2y1 = 4εu z
The kinematic Jacobian of Eq. (6.3.33) thus reduces to

éI3 B (p, a¢) A (p ) -s Px1 ù -s Py1
ê ú
ê0 w B (p, La¢) w1T A ( p ) L 2εu Tz A(p)La¢
T
1 0 ú
Fq = ê 0 w B (p, La¢) w T2 A ( p ) L
T
0 4εu z A (p)La¢ ú
T (6.3.35)
ê ú
2
ê0 pT 0 0 0 ú
ê0 0 a ¢T
L ú
ë 0 0 û
The velocity of contact point Pe in the ellipse, for the value of a¢ that defines the contact
point, is
v Pe = r& + A(p)w% ¢a¢ = r& + A(p) A T (p) A & (p)a¢
& (p)a¢ = r& + ( A(p)aˆ ¢ ) p& = r& + B(p, a¢)p& (6.3.36)
= r& + A p
The velocity of contact point Ps in the surface, for the values of x1 and y1 that define the contact
point in Eq. (6.3.26), is
v Ps = g(t)
& uz (6.3.37)
where u z is the unit vector along the positive z axis. The relative velocity of contact points Ps in
the ellipsoid and Pe in the surface is thus
v Pe - v Ps = r& + B(p, a¢)p& - g& u z (6.3.38)

In order that the projection of this relative velocity on the tangent plane to the surface at the point
of contact is zero; i.e., there is no relative slip between the ellipsoid and surface, it is necessary
that
w1T ( r& + B(p, a¢)p& - g& u z ) = 0
(6.3.39)
w T2 ( r& + B(p, a¢)p& - g& u z ) = 0
In matrix form, these differential constraints are
525
éw T w1T B(p, a¢) 0ù é2εx1 ù
E(q)q& = ê 1T ú q& = g& ê ú=e (6.3.40)
ëw 2 w 2 B(p, a¢) 0û
T
ë 4εy1 û
Derivative expressions required for formulation of the equations of motion are
( )
é w T B(p& , a¢)p& + ( B ( p,a¢ ) p& ) a& ¢ + 2εx& u T ( r& + B(p, a¢)p& )ù
( ) ê 1 ú
i a¢i 1 z
Eqˆ& q& = ê
( )
ú (6.3.41)
ê w 2 B(p& , a¢)p& + ( B ( p,a¢i ) p& )a¢i a& ¢ + 4εy& 1u z (r& + B(p, a¢)p& ) ú
q T T
ë û
é B(p& , a¢)p& + ( B ( p,a¢i ) p& ) a& ¢ + B(p& , a& ¢)p - 2ε(x& 1 )2 u z - 4ε(y& 1 )2 u z ù
a¢i
ê ú
ê
ê (
w1 B(p& , La¢)p& + B(p& , La& ¢)p + ( B ( p,a¢i ) p& )a ¢ La& ¢ + c1
T
) ú
ú
( )
i
ê ú
( )
ˆ
F qq q =
& &
w T2 B(p& , La¢)p& + B(p& , La& ¢)p + ( B ( p,a¢i ) p& )a¢ La& ¢ + c 2
(6.3.42)
q ê ú
ê i
ú
ê p& Tp& ú
ê ú
ë a& ¢ La& ¢
T
û
where, from Eq. (2.6.81),
( B ( p,a¢ ) p& )
i a¢i
= 2 ( E ( p ) GT ( p& ) )
c1 = 4εx& 1uTz ( B(p& , La¢)p + A(p)La& ¢) (6.3.43)

c 2 = 8εy& 1u Tz ( B(p& , La¢)p + A(p )La& ¢ )
Time derivatives of holonomic constraints of Eq. (6.3.32) are F qt = 0 and
é -g& u z ù
Ft = ê ú
ë 04´1 û
(6.3.44)
é -&&
gu z ù
F tt = ê ú
ë 04´1 û
Derivatives of the right side of the differential constraints of Eq. (6.3.40) are
é 2εx1 ù
et = ê ú &&
g
ë 4εy1 û
(6.3.45)
é 2εx& ù
eq q& = ê 1 ú g&
ë 4εy& 1 û
and Et = 0 . Finally, right side vectors in Eqs. (6.1.8) and (6.1.9) are
526
é -F ù
n =ê tú
ë e û
ê
(
é F qˆ& + F ù
q
q
) tt
ú
(6.3.46)
g =ê ú
( )
ˆ
êë Eq& q - eq q& - e t úû
Initial conditions q0 on q are readily defined to satisfy Eq. (6.3.32), using physical
reasoning. Similarly, initial velocity and angular velocity w¢0 are readily defined to be consistent
with the no-slip condition and p& 0 = 0.5G (p 0 )T w¢0 of Eq. (2.6.64) defines associated Euler
parameter time derivatives. Initial values for a& ¢, x& 1 , and y& 1 are not easily defined on physical
grounds. Solving the velocity equations of Eq. (6.1.6), augmented with specified initial
conditions at t 0 , resolves this issue.
With density r , the mass of a homogeneous ellipsoid is m = 4rpabd / 3 and its moments
of inertia J x¢x¢ = m(b2 + d2 ) / 5, J y¢y¢ = m(a 2 + d2 ) / 5, and J z¢z¢ = m(b2 + a 2 ) / 5 define the inertia
matrix as J¢ = diag(J x¢x¢ ,J y¢y¢ ,J z¢z¢ ) . As a numerical example, the density is selected as
r = 140 kg/m3 . With the centroidal body-fixed reference frame used, terms required in the
equations of motion are
M = blockdiag ( mI3 , 4G T (p)J¢G (p), I5 )
S = éê0, 0, 0, ( 8G T (p& )J¢G (p& )p ) , 0, 0, 0, 0, 0 ùú

T T
(6.3.47)
ë û
T
Q A = é( -mgu z - Kr ) , 0,0,0,0,0,0,0,0,0ù
T
ë û
where the generalized force is due to gravity, g = 9.8 m/ sec2 , that acts in the negative z-direction
and a spring with stiffness K N/m that acts between the centroid of the ellipsoid and the origin of
the x-y-z frame.
Simulations are carried out with MATLAB Code 6.3.2 of Appendix 6.A, using constant
step size RK4 and variable step size RKF45, with error tolerance e-5, to study the effects of the
geometry of the ellipsoid, initial angular velocity of the ellipsoid, motion of the surface, and
reducing the surface to the x-y plane. A body of literature on conservation properties of
dynamics of a body rolling without slip on a surface (Garcia-Naranjo and Marrero, 2013; Bloch.,
2003) suggests that conservation of momentum and other measures of dynamic response depend
on the ellipsoid having an axis of symmetry, which is not the case if α ¹ β ¹ δ . Checks are made
with simulation to evaluate such conservative properties.
Simulations are first carried out with α = 0.5, β = 0.4, δ = 0.3 , r = 140 kg/m3 , and K =
150 N/m; motion parameters amp = 0.1 m and w = om = 3 rad/sec; surface parameter ε = 0.1; and
the initial configuration
q 0 = éër 0T p 0T a¢0T x10 y10 ùû = [ 0, 0, 0.3,1, 0, 0, 0, 0, 0, -0.3, 0, 0]
T T
527
Initial velocity and angular velocity components are specified as x& 0 = y& 0 = 0.3 m/sec and
w0z¢ = 10 rad/sec. These conditions are appended to Eq. (6.1.8), to determine the complete set of
initial velocities, q& 0 = [0.3, 0.3, 0, 0, -0.5, -0.5, 5,1, 6.78, 0,1, 6.78]T . The initial integration step size
is selected as h = 0.005 sec, subsequently adjusted by the algorithm with an upper limit of 0.01
sec, and the simulation final time is t f = 10 sec. Results are shown in Fig. 6.3.8.
Figure 6.3.8 Simulations with Asymmetric Ellipsoid; α = 0.5, β = 0.4, δ = 0.3 m

Plots in Fig. 6.3.8 show x-y trajectories of the centroid of the ellipsoid. Captions on the
four plots indicate a range from motion of the surface with nonzero initial vertical angular
velocity of the ellipsoid at the upper left to rolling on the x-y plane with zero initial angular
velocity of the ellipsoid at the lower right. In the upper left simulation, both total energy and
vertical momentum vary, the former as a result of energy being introduced due to motion
imposed by the surface. In the upper right simulation, even though the initial vertical momentum
is zero, its value and that of total energy vary over time, due to motion of the surface. When
motion of the surface is turned off, as in the simulation at the lower left, total energy is conserved
but vertical momentum varies. With rolling on the x-y plane shown at the lower right, total
energy is constant, but vertical momentum varies. The variation of vertical momentum in all four
simulations is consistent with theoretical results (Garcia-Naranjo and Marrero, 2013; Bloch.,
2003) associated with an ellipsoid that has no axis of symmetry. As is clear from trajectories in
528
all four simulations, the effects of shape and motion of the surface and initial vertical angular
velocity of the ellipsoid lead to rather distinct motions.
The preceding four simulations are repeated for a sphere of radius α = β = δ = 0.4 m, with
results shown in Fig. 6.3.9. For motion of the surface and nonzero initial vertical angular velocity
in the simulation shown at the upper left, both total energy and vertical momentum vary with
time. Even with zero initial vertical angular velocity for the simulation shown at the upper right,
total energy and vertical momentum vary with time, due to shape and motion of the surface.
With motion of the surface turned off, as shown at the lower left, total energy is constant but
vertical momentum varies with time, likely due to the shape of the surface. Finally, as shown at
the lower right, with motion on the plane, a simple straight line trajectory occurs and both total
energy and vertical momentum are constant.
Figure 6.3.9 Simulations with Sphere; α = β = δ = 0.4 m

In the most severe simulation at the upper left of Fig. 6.3.4, 39 reparameterizations were
required in 1970 time steps (50 time steps per reparameterization), all due to a unit limit on norm
of v. In most other simulations, only one parameterization was required for 2,000 time steps. In
all cases, computing cost for reparameterization was negligible.
As a check on constraint error control, the tolerance Tol = utol = Bto l = Htol in Eqs.
(6.1.19), (6.1.21), and (6.1.33) is varied for the most severe simulation at the upper left of Fig.
529
6.3.5, carried out with the RK4 constant step size method. Results are shown in Table 6.3.3. Data
in the table show that position, velocity, and acceleration constraint errors are driven toward
zero, to computer precision, as tolerances are tightened. There was little increase in computing
times associated with the tighter tolerances.
Table 6.3.3 Constraint Error for Ellipsoid on Moving Surface
e-6 e-6 e-8 e-6
e-9 e-8 e-14 e-13
e-12 e-11 e-14 e-13
A simple planar vehicle example, two spatial models of intermediate complexity, and a rather
intricate spatial example of mixed holonomic-nonholonomic systems are presented, using
expressions derived for derivatives that are required for implementation of explicit integration
algorithms. Solutions obtained using MATLAB codes of Appendix 6.A are shown to satisfy all
three forms of kinematic constraint and yield practical solutions. While the explicitly derived
spatial examples involve intricate expressions, it is clear that a computer implementation with
standard modeling elements is possible.
530
6.4 Index 0 DAE Formulation for Nonholonomic Systems
As with holonomic systems in Chapter 5, the nonholonomic ODE of Section 6.2 are
converted to an equivalent Index 0 DAE, through introduction of Lagrange multipliers that are
available for computation of constraint reaction forces. Explicit and implicit numerical
integration methods of Section 4.8 are adapted for solution of the DAE.
6.4.1 Index 0 DAE for Nonholonomic Systems
( )
Substituting K T = W T I - CT HT XT from Eq. (6.1.36) into the second of Eqs. (6.2.5)
and using Q º MXHg + QA + S to simplify notation,
& - WT CT HT XT MKw
W T MKw & = WT Q - WT CT HT XTQ (6.4.1)
Defining
l º -H T XT ( MKw
& + Q) (6.4.2)
Eq. (6.4.1) becomes

W T ( MKw
& + Cl - Q ) = 0 (6.4.3)
Multiplying Eq. (6.4.2) on the left by X T CT yields

X TCT l = - XT CT H T X T ( MKw
& - Q)
Taking the transpose of HCX = I from Eq. (6.1.31), X T CT H T = I and this reduces to
XT ( MKw
& + CT l - Q ) (6.4.4)
Combining Eqs. (6.4.3) and (6.4.4)
[W X ] ( MKw
& + CT l - Q ) = 0
T
Since the columns of W and X span R ngc , [ W X ] is nonsingular and
& + CT l - Q = 0
MKw
Substituting the above definition of Q, this is
& + CT l - ( MXHg + QA + S ) = 0
MKw (6.4.5)
Equation (6.4.5) is a DAE, since it involves the algebraic variable l . The foregoing
manipulations are reversible, so the DAE may be reduced to the ODE of the second of Eqs.
(6.2.5), without taking any derivatives. By definition of differentiation index (Ascher and
Petzold, 1998), Eq. (6.4.5) is an Index 0 DAE, as is the first order system obtained by combining
it with the kinematic differential equation of Eq. (6.1.38); i.e.,
(6.4.6)
& + CT l = MXHg + Q A + S
MKw
Writing Eqs. (6.4.6) in matrix form,
531
é v& ù
éI 0 0 ù ê ú é V T Kw + V T (I - XHC)q& 0 + V T XHν ù
ê w
Túê ú
& =ê ú (6.4.7)
ë 0 MK C û ê l ú ë MXHg + S + Q A û
ë û
for existence of a unique solution, the coefficient matrix on the left must be nonsingular. At a
parameterization point (q0 , t 0 ) that satisfies Eqs. (6.1.1), Eqs. (6.1.25) and (6.1.36) show that
K 0 = W . To show that the coefficient matrix on the left of Eq. (6.4.7) at (q0 , t 0 ) is nonsingular
with K 0 = W and C0T = X , set
éα ù
éI 0 0ùê ú
ê 0 MW X ú ê β ú = 0 (6.4.8)
ë û êδ ú
ë û
If this equation holds only for α = 0, β = 0, and δ = 0 , then the coefficient matrix is nonsingular.
The first row of Eq. (6.4.8) is α = 0 and the second is MWβ + Xδ = 0 . Multiplying both sides
on the left by WT , W T MWβ + W T Xδ = W T MWβ = 0 . It is shown in Section 4.6.2 that M is
positive definite on the null space of C , so the matrix W T MW is positive definite, hence
nonsingular, and β = 0 . The second of Eqs. (6.4.8) thus reduces to Xδ = 0 and, since X has full
column rank, δ = 0 . This shows that the matrix on the left of Eq. (6.4.8) is nonsingular. The
coefficient matrix of Eq. (6.4.7) is thus nonsingular at (q0 , t 0 ) and, since functions in the matrix
are continuous in q and t, the coefficient matrix on the left of Eq. (6.4.7) is nonsingular in a
neighborhood of (q0 , t 0 ) . This result is the basis for applying explicit and implicit ODE
integrators for numerical solution of the Index 0 DAE of Eqs. (6.4.6).
T
Define y = éë v T w T ùû and
é f1 ( t,y) ù é V Kw + V (I - XHC)q& + V XHν ù

T T 0 T
f ( t,y) = ê ú=ê ú (6.4.9)

( ) ( )
-1
ëf 2 ( t,y) û êë K MK K MXHg + S + Q úû
T T A
where Eqs. (6.1.22) and (6.1.35) define q and q& as functions of v and w, hence y. The first order
ODE of Eq. (6.2.5) are thus y& = f ( t,y ) , which is equivalent to the Index 0 DAE of Eq. (6.4.6).
Initial conditions of Eqs. (6.2.6) are
é v0 ù
y0 = ê 0 ú = 0 (6.4.10)
ëw û
The first order initial-value ODE problem is thus
y& = f ( t,y )
(6.4.11)
y0 = 0
532
6.4.2 Explicit Integration of Nonholonomic Index 0 DAE
Explicit integration of the Index 0 DAE initial-value problem of Eqs. (6.4.6) and
(6.4.10), using explicit Runge-Kutta methods, is as follows:
(1) With initial conditions q 0 and q& 0 at t 0 that satisfy Eqs. (6.1.1) and (6.1.8), use singular
value decomposition to determine V and W that satisfy Eqs. (6.1.13) and (6.1.25). Evaluate
B and H of Eqs. (6.1.20) and (6.1.31) at t 0 and evaluate U and X of Eqs. (6.1.13) and
(6.1.26).
& i , v& i , and l i and apply an explicit Runge-Kutta
(2) At time step t i , solve Eq. (6.4.7) for w
numerical integrator to obtain w i +1 and v i +1 . Evaluate B using Eq. (6.1.21) and H using Eq.
(6.1.33). Use Eqs. (6.1.22), (6.1.35), and (6.1.39) to evaluate q, q& , and q && on the time grid.
(3) Monitor the condition number of the coefficient matrix on the left of Eq. (6.4.7), the
norms of v and w, and the number of iterations required to evaluate u, B, and H. If
tolerances are exceeded, define a new time t 0 and associated q 0 and q& 0 . Repeat Step (1) to
define a new parameterization and initial conditions v 0 and w 0 . This process follows the
trajectory shown in Fig. 5.2.2, moving smoothly across the constraint set.
A key aspect of this algorithm is control of error in satisfying constraints of Eqs. (6.1.1),
(6.1.8), and (6.1.9). These errors can be controlled with adequate tolerances utol, Btol, and Htol
in iterative solution for u, B, and H. While the value of λ i determined in Step (2) is not used in
integration, it is available for computation of constraint reaction forces.
6.4.3 Derivatives for Implicit Numerical Integration
To use an implicit numerical integration method in solving Eqs. (6.4.6) and (6.4.10),
derivatives of all terms appearing in Eq. (6.4.6) with respect to v, w, and l are required. For the
reader who is satisfied with the use of explicit numerical integration methods, the somewhat
intricate remainder of this section can be bypassed.
If Eqs. (6.4.6) are to be solved numerically with an implicit numerical integration
method, a convenient form is the residual equation
é R1 ù é v& - V T Kw - V T (I - XHC)q& 0 - V T XHν ù
Rºê ú=ê ú=0 (6.4.12)
ëR 2û ë MKw & + CT l - MXHg - S - Q A û
This form of the equations of motion in terms of &v, w & , l , q, and &q may be used to iteratively
solve discretized equations that are based on implicit numerical integration formulas. To
implement this approach, expressions for derivatives of terms in Eq. (6.4.12) with respect to
& , v, w,and l must be derived. Fortunately, most terms in Eqs. (6.4.12) appear in Eq. (6.2.5),
v& , w
so derivative terms presented in Section 6.2 are applicable here. The only term in Eqs. (6.4.12)
that does not appear in Eqs. (6.2.5) is CT l . Fortunately, its derivative with respect to q is the
term
533
(
P4(q, l ) º CT (q, ˆt)lˆ ) q
(6.4.13)
defined in Eq. (5.3.19) and expanded for computation in Eq. (5.3.35).

The easy part of the Jacobian of the residual in Eq. (6.4.12) is
éI 0 0 ù
R v& ,w& ,l = ê Tú
(6.4.14)
ë0 MK C û
Kinematic derivatives q v , q w , q& v , and q& w are identical to Eq. (6.2.12), repeated here as
q v = V - Uh v
qw = 0
q& v = ( ( Kwˆ )q - X ( HCq

ˆ ˆ& 0 ) - XH ( Cqˆ& 0 ) + X ( Hνˆ ) + XHν
q q q q ) qv
(6.4.15)
q& w = K
Using these results, derivatives of the residual of Eq. (6.4.12) are
(
R1v = - V T ( Kw
ˆ )q + V T X HCq
ˆ &ˆ 0 ( ) q
(
+ V T XH Cq&ˆ 0 ) q
- V T X ( Hnˆ )q - V T XHνq q v)
R 1w = - V T K
( &ˆ
R 2 v = M Kw ( ) + ( MKw
q
ˆ &ˆ ) - ( MXH
ˆ ˆ gˆ )
q q
- MX ( Hgˆ )q - MXHg q - QqA - Sq q v ) (6.4.16)
+ P4(q, l ) - ( MXHg q& + QqA& + Sq& ) q& v

R 2 w = - ( MXHg q& + QqA& + Sq& ) q& w
( )
Derivative expressions for ( Caˆ )q , Hbˆ , and ( Kcˆ )q are given in Eqs. (6.2.15), (6.2.16), and
q
(6.2.17); g , g q , and g q& are given in Eqs. (6.2.19) and (6.2.22); and kinetic derivatives
( M(q)mˆ )q , Sq , Sq& , QqA , and QqA& are presented in Appendix 5.B. These results enable evaluation of
all terms in Eq. (6.4.16).
6.4.4 Implicit Numerical Integration Algorithms
Implicit numerical integration algorithms of Section 4.8.1 for solution of first order ODE
are applied for solution of Eqs. (6.4.6) and (6.4.10). Since trapezoidal integration formulas are
stated in terms of generalized coordinates, their application is rather simple, as presented in
Section 6.4.4.1. Implicit Runge-Kutta integration formulas, in contrast, are stated in terms of
stage variables that are not generalized coordinates. They are applied to the explicit ODE of Eq.
(6.4.11) and transformed for solution of Eq. (6.4.6) in Section 6.4.4.2.
6.4.4.1 Trapezoidal Integration
Implicit trapezoidal integration formulas for v& and w& of Eq. (4.8.40) are
534
v n = v n -1 + (h / 2) ( v& n -1 + v& n )
(6.4.17)
w n = w n -1 + (h / 2) ( w & n)
& n -1 + w
These equations are used with Eqs. (6.1.22) and (6.1.35) to evaluate q and q& as functions of
v& n and w
& n , for use in Eq. (6.4.12), yielding
é v& - V T K ( w n -1 + (h / 2) ( w & n ) ) - V T (I - XHC)q& 0 - V T XHν ù

& n -1 + w
& n) = ê n
R ( v& n , w ú=0
ëê MKw & n + C T l - MXHg - S - Q A ûú
(6.4.18)
Equations (6.4.17) allow all arguments of functions that appear in Eq. (6.4.18) to be written as
functions of v& n and w
& n . Using the chain rule of differentiation, with relations derived in Section
T
6.4.2, the Jacobian of the residual of Eq. (6.4.18) with respect to z n º éë v& Tn & Tn
w l Tn ùû is
éI 0 0ù é R1 R1w 0ù
J trap = ê + (h / 2) ê v º J1 + (h / 2)J 2
Tú
0 úû
(6.4.19)
ë 0 MK C û ë R 2v R 2w
At t 0 , with h small, the first matrix dominates. As shown in Section 6.4.1, it is nonsingular, so
the Jacobian is nonsingular in a neighborhood of q0 , for h sufficiently small.
Newton-Raphson iteration for solution of Eq. (6.4.18) is
J trap Dz i = - R i
i = 0,1,... until R i < intol (6.4.20)
z i +1 = z i + Dz i
At t 0 , with initial conditions of Eq. (6.4.10) and h = 0, Eq. (6.4.18) is solved to obtain an
T
estimate z 0 º éë v& 0T w
& 0T ùû to start the numerical integration process. At subsequent time steps,
good estimates for z0n are available from prior time steps. Bounds on the norms of v and w, the
number of iterations in evaluating u, B, and H, and the condition number of the coefficient
matrix of Eq. (6.4.20) are used to determine whether a reparameterization is required. If not, the
integration process is continued. If so, the current time is set as t 0 and the current value of q is
used as q 0 to redefine V, U, W, X, B, and H . The process is restarted with initial conditions
v 0 = 0 and w 0 = 0 and estimates for their time derivatives v& 0 = V T q& 0 and w
& 0 = WTq
&& 0 . The
T
estimate z n 0 º éë v& 0T
n
& 0T
w n l 0T
n û
ù is used to evaluate the Jacobian of Eq. (6.4.19) and restart the
numerical integration process.
6.4.4.2 SDIRK Integration
Runge-Kutta numerical integration methods are presented in Section 4.8.1.1 for solution
of first order ODE of the form
y& = f ( t,y ) (6.4.21)
535
where y is an n-vector variable. A variety of RK integrators can be written in the form
æ i ö
k i = f ç t n + ci h,y n + h å a ijk j ÷ , i=1,...,s (6.4.22)
è j=1 ø
s
y n +1 = y n + h å bik i (6.4.23)
i=1
i
j=1
s
are constants; åb
i =1
i = 1 ; s is the number of stages; k i are stage variables; and h is the step-size. If
diagonal terms of the matrix a = [a i j ] are a ii = a ¹ 0 , i=1,2,…,s, the method is called Singly
Diagonal, or SDIRK. The stiffly-accurate, L-stable, 5 stage, order 4 SDIRK54 formula for first
order ODE is defined in Table 4.8.6.
To integrate the first order ODE of Eq. (6.4.11) using a Runge-Kutta method, stage
T
variables are k i = éëkviT kw iT ùû and arguments of functions in the stage equations are
q ( v) and q& ( v, v& ) . Evaluated as functions of stage variables, they are
æ i ö
qi = q ç v n + h å a i jkv j ÷
è j=1 ø
(6.4.24)
æ i i
ö
q& i = q& ç v n + h å a i jkv j , w n + h å a i jkw j ÷
è j=1 j=1 ø
Denote a general function g (t, v, w ) that is evaluated as a function of stage variables as
( æ i i ö
g i = g ç t n + ci h,v n + h å a i jkv j , w n + h å a i jkw j ÷ (6.4.25)
è j=1 j=1 ø
With this notation, stage equations for the ODE of Eqs. (6.4.11), in residual form, are
( æ( i ( ( ( ( ö
R1 º kv i - V ç D i ( w n + h å a i jkw j ) + ( I - XHi Ci )q& 0 + XHi ν i ÷ = 0
T
(6.4.26)
è j=1 ø
(( ( ( ( -1 ( ( ( ( ( (
( ) (
R 2 º kw i - K Ti M i K i K Ti MXHg + Q A + S = 0 ) i
(6.4.27)
Repeating the manipulations of Eqs. (6.4.1) through (6.4.6) reduces Eq. (6.4.27) to
( ( ( ( ( (( ( (
R 2 = ( M i K i ) kw (
& i + Ci Tl i - MXHg + Q A + S = 0 )
(6.4.28)
i
Equations (6.4.26) and (6.4.28) comprise the residual form of Runge-Kutta discretization
of Eq. (6.4.18). Using derivatives of residual terms in Eqs. (6.4.15) and (6.4.16) and the chain
rule of differentiation with Eq. (6.4.25), the Jacobian of Eqs. (6.4.26) and (6.4.28) for an SDIRK
T
integrator with a ii = a , in variables z i = éëkv iT kw iT λ i ùû , is
536
( (
éI 0 0ù é R1v R1w 0ù ( (
J RK
=ê ( ( ( + hα ê ( ( ú º J1 + hαJ 2
CiT úû
(6.4.29)
ë0 M i K i ë R 2v R 2w 0û
The Newton-Raphson algorithm is used with the same approach outlined for the
trapezoidal method to obtain initial estimates to start the iterative process and to redefine the
parameterization and restart integration, as required.
6.4.4.3 Implicit Integration Algorithm
Implicit Numerical integration Eqs. (6.4.6) and (6.4.10), using trapezoidal and Runge-
Kutta methods is as follows:
velocity constraints. Evaluate the constraint Jacobian F q (q 0 , t 0 ) and matrices U and V in
Eqs. (6.1.13) and the system velocity coefficient matrix C(q 0 , t 0 ) and matrices W and X in
Eqs. (6.1.25) and (6.1.26).
determine z n . Use Eq. (6.4.17) or (6.4.23) to determine v n and w n . Use Eqs. (6.1.22),
(6.1.35), and (6.1.39) to evaluate q n , q& n , and q
&& n on the time grid.
(3) Monitor the condition number of J RK or J trap , the number of Newton-Raphson iterations
required in Step (2), the norms of v and w, and the number of iterations required to evaluate
u, B, and H. If tolerances are exceeded, define a new time t 0 and associated q0 and q& 0 .
Repeat Step (1) to define a new parameterization and initial conditions v 0 and w 0 .
With straightforward adaptation of the ODE formulation of Section 6.2, a first order Index 0
DAE involving Lagrange multipliers is obtained. Numerical integration formulas are applied to
an underlying ODE and inflated for solution with Lagrange multipliers, which are available for
computation of constraint reaction forces.
Key Formulas
v& = V T Kw + V T ( I - XHC ) q& 0 + V T XHn é v0 ù
ê 0ú = 0 (6.4.6) (6.4.10)
& + CTl = MXHg + Q A + S
MKw ëw û
537
6.5 Numerical Examples with Index 0 DAE
6.5.1 Planar Articulated Vehicle
The two chassis articulated vehicle treated with the tangent space ODE formulation in
Section 6.3.2 is shown in Fig. 6.5.1. It is treated here using the Index 0 DAE formulation. The
front axle of chassis one is steerable, with angle q(t) . Since roll without slip conditions for both
front wheels would be redundant, a single wheel model is placed at the center of the front axle at
point P1 and the roll without slip condition is applied to this wheel model. The chassis are
coupled by a revolute joint at point P3 that is orthogonal to their common plane of motion, 3 m
from the origins of body-fixed reference frames.
Figure 6.5.1 Planar Articulated Vehicle

Six generalized coordinates for the system are
f2 ùû = [ x1 f2 ]
T
q = éër1T f1 r2T y1 f1 x 2
T
y2
The revolute joint between the chassis is characterized by the holonomic constraint
Φ(q) = ( r1 - 3A(f1 ) i1¢ ) - ( r2 + 3A(f2 ) i ¢2 ) = 0 (6.5.1)
whose Jacobian is
Φq (q ) = [ I 2 -3PA(f1 )i1¢ -I2 -3PA(f2 )i ¢2 ] (6.5.2)
where A(fi ) is the rotation transformation matrix from the x i¢ -yi¢ frame to the x-y frame and i i¢
are unit vectors along the x i¢ axes.
Vectors from origins of body-fixed reference frames to centers of axles are
r = r1 + 3A ( f1 ) i1¢ , r1P3 = r1 - 3A ( f1 ) i1¢ , and r2P2 = r2 - 3A ( f2 ) i¢2 . The velocities of these points
1
P1
are
538
r&1P1 = r&1 + 3f& 1PA ( f1 ) i1¢
r& P3 = r& - 3f& PA ( f ) i¢
1 1 1 1 1 (6.5.3)
r&2P2 = r&2 - 3f& 2 PA ( f2 ) i¢2
where P = A( p / 2) and A(f)f = PA(f) .
The velocities of Eqs. (6.5.3) must be orthogonal to unit vectors u1 , u3 , and u 2 ,

respectively, as conditions that the wheels do not slip laterally; i.e.,
1 1 ( 1 1 1
T
)
u Tr& P1 = [ - sin q cos q] A ( f ) r& + 3f& Pi¢ = 0 , u T r& P3 = j¢T A ( f ) r& - 3f& = 0 , and
T
3 1 1 1 1 1
u 2 T r&2P2 = j¢2 T A ( f2 ) r&2 - 3f& 2 = 0 , where A T PA = P has been used. Differential constraints that
T
act on the system are thus

é[ - sin(q) cos(q)] A (f1 )T 3 [ - sin(q) cos(q) ] j1¢ 0 0ù
ê ú
E(q, t)q& = ê j1¢T A (f1 ) T -3 0 0 ú q& = 0
ê 0 0 j¢2 A(f2 )T
T
-3úû
ë
(6.5.4)
so e(q, t) = 0 . The composite constraint coefficient matrix of Eqs. (6.5.2) and (6.5.4) is
é Φ (q ) ù
C(q, t) = ê q ú (6.5.5)
ë E(q, t) û
As defined in Section 6.4, and in turn in Section 6.2, derivative terms needed for
T
numerical integration of the equations of motion, with χ = éë χ rT1 χ f1 χ Tr2 χ f2 ùû , are
F t = - ν = 0, F tq = F ttq = 0, and
(
P2( q, χ ) = Φ q ( q, tˆ ) χˆ ) q
= éë 0 3χ f1 A( f1 )i1¢ 0 3χ f2 A(f2 )i¢2 ùû
(
P3(q, q& ) = P 2(q, q&ˆ )q&ˆ ) q
= éë0 3f& 12 PA(f1 )i1¢ 0 3f& 22 PA (f2 )i¢2 ùû
& é[cos q sinq]A(f1 )T 3[cos q sinq]j1¢ 0 0 ù

Et = - q ê ú
ë 0 2´6 û
é 0 [cos q sinq]A(f1 )T Pr&1 0 0ù
( E t ( q, ˆt)qˆ& ) q
&
= qê
ë 02´6
ú
û
é 0 -[ - sin( q) cos(q)]A(f1 )T Pχ r1 0 ù 0
ê ú
(
E2(q, χ ) = E(q, tˆ )χˆ ) q
= ê0 - j1¢T A(f1 )T Pχ r1 0 0 ú
ê0 0 0 - j¢2 A(f2 ) Pχ r2 úû
T T
ë
539
é 0 - f& 1 [ - sin(q) cos(q) ] A(f1 ) T r&1 0 0 ù
(
E3(q, q& ) = E2(q, q&ˆ )q&ˆ ) q
ê
= ê0 - f& 1 j1¢ A(f1 ) r&1
T T
0 0
ú
ú
ê0 0 0 - & j¢ T A (f )T r& ú
f
ë 2 2 2 2û
( C (q, tˆ )lˆ )
T
q
= éë 0 6´ 2 C fT1 l 0 6´ 2 CfT2 l ùû
where
é 0 3A (f1 )i1¢ 0 0ù
ê - - sin(q) cos(q) A T (f )P 0 úú
[ ] 0 0
Cf1 = ê 1
ê - j1¢ A (f1 )P
T T
0 0 0ú
ê ú
ë 0 0 0 0û
é0 0 0 3A (f2 )i¢2 ù
ê0 0 0 0 ú
Cf2 = ê ú
ê0 0 0 0 ú
ê ú
ë 0 0 - j¢2 A (f2 )P
T T
0 û
With these results, g , g q , and g q& of Eqs. (6.2.19) and (6.2.22) may be evaluated. In the planar
equations of motion for this system, M = mI 6´6 , S = 0, and Q A = 0 .
With two holonomic and three differential constraints on six generalized coordinates and
velocities, this system has four kinematic degrees of freedom and one kinetic degree of freedom.
Accordingly, there are four first order kinematic equations for v and one first order kinetic
equation for w in Eqs. (6.4.6).
Numerical results obtained with the Index 0 formulation using MATLAB Code 6.5.1 of
Appendix 6.A, with explicit RK4 and implicit trapezoidal and SDIRK54 algorithms, yielded
essentially identical results to those reported in Section 6.3.2 using the ODE formulation.
6.5.2 Spatial Disk Rolling Without Slip on x-y Plane
The disk with unit radius shown in Fig. 6.5.3 that rolls without slip on the x-y plane is
treated in section 6.3.3 with the tangent space ODE formulation It is treated here using the Index
0 DAE formulation. The plane of the disk is defined by body fixed y¢-z¢ axes, where the body
fixed x ¢ axis is normal to the plane of the disk. Unit vector a¢ in the x¢-y¢-z¢ frame from the
center of the disk to contact point C on the periphery with the x-y plane is
é0ù
é0 ù é0 ù
a I
and the vector a = [ a1 a 2 ]

T
satisfies the condition
( a a - 1) / 2 = 0
T
(6.5.7)
540
The normal to the disk periphery at contact point C in the plane of the disk is the vector a¢ , so
é 0 ù
é 0 ù é0ù
b¢ = êê -a 2 úú = ê ú = ê ú a º b¢a a (6.5.8)
Pa P
Figure 6.5.3 Disk Rolling Without Slip on the x-y Plane

Euler parameters are used as generalized coordinates for orientation. Contact conditions
are that point C on the disk is in the x-y plane, u Tz ( r + A(p)a¢aa ) = 0 , and that the vector
b = A (p )b¢ is in the x-y plane, u Tz A(p)b¢aa = 0 . Combining these conditions, Eq. (6.5.7), and the
Euler parameter normalization condition yield the holonomic constraints
éu Tz ( r + A(p)a¢a a ) ù
ê ú
ê u Tz A(p)b¢a a ú
F(q) = =0 (6.5.9)
ê (p Tp - 1) / 2 ú
ê ú
ë (a a - 1) / 2 û
T
T
where generalized coordinates are q = éër T pT a T ùû Î R 9 . Since F(q ) does not depend
explicitly on t, F t = - ν h = 0, F tt = 0, and Fqt = 0 . The holonomic constraint Jacobian is
éuTz u Tz B(p,a¢a a) u Tz A(p)a¢a ù

ê ú
ê 0 u Tz B(p, b¢a a) u Tz A(p)b¢a ú
Fq = (6.5.10)
ê0 pT 0 ú
ê ú
ë0 0 aT û
541
v C = r& + A(p)ω &
% ¢a¢aa = r& + A(p)A T (p)A(p)a ¢a a
(6.5.11)
&
= r& + A(p)a¢ a = r& + ( A(p)a¢ aˆ ) p& = r& + B(p,a¢ a)p&
a a p a
components of v C are zero; i.e., u Tx v c = 0 and u Ty v c = 0 . With Eq. (6.5.11), this is
éu T u xT B(p,a¢a a) 0 ù
E(q)q& º ê Tx ú q& º e(q, t) = 0 (6.5.12)
êëu y u yT B(p,a¢a a) 0 úû
Since E does not depend explicitly on t and e = 0 ; Et = 0, eq = 0, and e t = 0 . With four

holonomic and two differential constraints on nine generalized coordinates, the system has five
kinematic degrees of freedom and three kinetic degrees of freedom.
Kinetic and force terms in the equations of motion are
é mI 0 0ù
M(q) = ê 0 4G (p)J¢G(p) 0 úú
ê T
êë 0 0 0 úû
é 0 ù
ê
& = 8G (q)J
S(q,q) T
& ú
& ¢G(q)p (6.5.13)
ê ú
êë 0 úû
é - mgu z ù
Q = êê 0 úú
A
êë 0 úû
Derivatives of terms that are required for integration of the equations of motion, with
T
χ = éë χ Tr χ pT χ aT ùû , are
é 0 u zT B(χ p , a¢a a) + u zT B(p, a¢a χ a ) u Tz M (p, χ p )a¢a ù

ê ú
0 u Tz B(χ p , b¢a a) + u Tz B(p, b¢a χ a ) uTz M (p, χ p )b¢a ú
P 2(q, χ ) = ( F q (q)χˆ )q =ê
ê0 χ pT 0 ú
ê ú
ëê 0 0 χ aT ûú
é 0 uTz B(p& , a¢a a& ) + u Tz Z (p& , a¢a a& ) u Tz M (p& , p& )a¢a ù
ê ú
( ( ))
P3 ( q, q& ) = P 2 q, qˆ& qˆ&
q
= ê
ê0
0 u Tz B(p& , b¢a a& ) + u Tz Z (p& , b¢a a& ) uTz M (p& , p& )b¢a ú
0 0 ú
ê ú
ë0 0 0 û
where B(×, ×), M(×, ×), and Z(×, ×) are defined in Eqs. (2.6.25), (2.6.81), and (2.6.83),
é 0 u Tx B(χ p , a¢a a) u Tx M (p, χ p )a¢a ù

E2(q, χ ) = ( E(q)χˆ )q =ê ú
ëê 0 u y B(χ p , a¢a a) u y M (p, χ p )a¢a ûú
T T
542
é0 u Tx Z(p& , a¢a a& ) u Tx M (p& , p& )a¢a ù
(
E3(q, q& ) = E2(q, q&ˆ )q&ˆ ) q
=ê T T ú
êë0 u y Z(p& , a¢a a& ) u y M (p& , p& )a¢a úû
é0 0 0 ù
(
C (q)λˆ = 0
T
)
q
ê
ê
α1 α2 ú
ú
êë 0 l1a¢a T C(p, u z ) + l 2b¢a T C(p, u z ) l 4 I 2 úû
α1 = l1K (a¢a a, u z ) + l 2 K (b¢a a, u z ) + l 3I 4 + l 5K (a¢a a, u x ) + l 6 K (a¢aa, u y )
α 2 = l1CT (p, u z )a¢a + l 2 CT (p, u z )b¢a + l 5CT (p, u x )a¢a + l 6 CT (p, u y )a¢a
é0 0 0ù
ê ú
M 2(q, μ) = ( M (q)μˆ )q = ê0 4 ( T( J ¢G (p)μ p ) - G T (p) J¢G (μ p ) ) 0 ú
ê0 0 0 úû
ë
é0 0 0ù
Sq = ê 0 8G (p& )J¢G (p& ) 0 úú
ê T
êë 0 0 0 úû
é0 0 0ù
Sq& = ê 0 8T(J¢G (p& )p) - 8G (p& )J ¢G (p) 0 úú
ê T
êë 0 0 0 úû
and QqA = QqA& = 0 , where K (×, ×) and T(×) are defined in Eqs. (2.6.37) and (2.6.20).
The formulation was implemented in MATLAB Code 6.5.2 of Appendix 6.A, using
explicit RK4 and implicit SDIRK54 integration methods. Results obtained with the Index 0 DAE
formulation were essentially identical to those reported in Section 6.3.3 using the ODE
formulation.
A planar articulated vehicle with wheels that roll, but do not slip, and a disk that rolls without
slip on a plane in three dimensional space are successfully simulated using the Index 0 DAE
formulation. Accurate solutions are obtained using MATLAB codes of Appendix 6.A. All
three forms of kinematic constraint are satisfied to within specified tolerances.
543
6.6 Well Posed Formulations of Nonholonomic Mechanical System Dynamics
An abbreviated treatment of well posedness of the nonholonomic equations of multibody
dynamics is presented in this section. Details of dependence of the equations of motion on design
variables is essentially identical to that of Section 5.11 for holonomic systems, so it is not
repeated here. A more detailed development and proofs of results presented in this section may
be found in (Haug, 2020c)
The tangent space ODE initial-value problem for nonholonomic systems of Sections 6.1
and 6.2 is as follows:
v& = V T K (q, t)w + V T ( I - XH(q, t)C(q,t) ) q& 0 + V T XH(q, t)n(q, t)
(6.6.1)
& = ( K T (q, t)M(q )K (q, t) ) K T (q, t) ( M(q )XH(q, t) g + Q A + S )
-1
w
v0 = 0
(6.6.2)
w0 = 0
For simplicity of notation, explicit dependence on design variables b that is detailed in Section
5.11 is suppressed.
Assuming that all functions involved have k ³ 1 continuous derivatives with respect to q,
b, and t, Eqs. (6.6.1) and (6.6.2) comprise an ODE initial-value problem that is well posed
(Teschl, 2012). As in the holonomic case, the tangent space ODE and Index 0 DAE are
algebraically equivalent, so both have the same existence, uniqueness and regularity properties.
This justifies the following result:
Theorem 6.6.1: Well Posed Nonholonomic ODE If
(1) all functions that appear in Eq. (6.6.1) are k ³ 1 times continuously differentiable on a
bounded domain D of q -q& -b -t space,
(2) C(q, b,t) has full rank in D,
(3) M( q, b ) is positive definite on the null space of C((q, b,t) in D, and
(4) q0 (b) and q& 0 (b) satisfy Eqs. (6.1.1) and (6.1.6), respectively, at t 0 ,
then Eqs. (6.6.1) and (6.6.2) have a unique solution ( v (t,b ),w (t,b ) ) that is k times
continuously differentiable with respect to t and b in a neighborhood of (v0 ,w 0 ,b,t 0 ) .
Equations (6.1.20), (6.1.33), and (6.1.37) define a unique solution, in a neighborhood of
(q0 , q& 0 , b,t 0 ) , of the variational equations of motion of Eqs. (6.2.1), and (6.1.1) through
(6.1.8) that is k times continuously differentiable with respect to t and b.
Since existence, uniqueness, and continuity of solution of Eqs. (6.6.1) and (6.6.2) follow
from ODE theory of Section 4.7.3, it remains only to show that q, q& , and && q determined by Eqs.
(6.1.22), (6.1.35), and (6.1.39) satisfy the constraints of Eqs. (6.1.1), (6.1.8), and (6.1.9) and
initial conditions q(t 0 ) = q 0 and q& (t 0 ) = q& 0 satisfy Eqs. (6.1.1) and (6.1.8).
By construction, q, q& , and && q satisfy Eqs. (6.1.22), (6.1.35), and (6.1.39). At t 0 , Eqs.
(6.1.18) show that u0 = h( v 0 , b,t 0 ) = 0 , so q( v 0 , b,t 0 ) = q0 (b) , hence satisfying configuration
544
initial conditions, and Eq. (6.1.35) and K (q 0 , b,t 0 ) = 0 show that q& (v 0 , w 0 , b,t 0 ) = q& 0 (b ) . It
remains to show that Eq. (6.2.1) is satisfied for all δq that satisfy Eq. (6.1.8). Since the equation
following Eq. (6.2.2) holds for all a and δq = Ka , Eq. (6.2.1) holds for all δq such that
Cδq = CDa = 0 . This completes the proof.
Theorem 6.6.2 Well Posed Index 0 DAE Under the hypotheses of Theorem 6.6.1, the
Index 0 DAE formulation of Eqs. (6.4.6) with initial conditions of Eq. (6.6.2) is well
posed.
The proof follows by simply noting that the ODE and Index 0 DAE formulations are
equivalent.
Derivation of the well posed initial-value problem of Eqs. (6.6.1) and (6.6.2) shows that it
is satisfied by v(t, b ) and w (t, b ) that are defined by q(t, b ) that satisfies Eq. (6.2.1), for all δq
that satisfy Eq. (6.1.8), the constraints, and the initial conditions; i.e., by every solution of the
d’Alembert variational equations. Conversely, Theorem 6.6.1 shows that q, q& , and && q satisfy the
d’Alembert variational equations. If a solution of the variational equations is unique, the ODE
initial-value problem and d’Alembert variational equations are equivalent, hence both are well
posed, leading to the following result.
Theorem 6.6.3: Well Posed d’Alembert Nonholonomic Variational Equations Under
the hypotheses of Theorem 6.6.1, the d’Alembert variational equations are well posed.
To prove the theorem, it remains only to show that a solution of the d’Alembert equations
is unique. For purposes of obtaining a contradiction, assume there are solutions q ¹ q of the
variational equations, constraints, and initial conditions. As shown, they define solutions
( v,w) and ( v, w ) of the initial-value problem of Eqs. (6.6.1) and (6.6.2). From Eq. (6.1.22),
q - q = V(b) ( v - v ) - U (b ) ( h( v, b,t) - h( v, b,t) )

Since the solution of the ODE initial-value problem is unique, v = v , hence, q - q = 0 . This
yields the desired contradiction, which completes the proof.
The full DAE formulation is
&& + CTλ - Q A (q, q& , t) - S(q, q& ) = 0
M(q )q
q(t 0 ) = q0
q& (t 0 ) = q& 0
F(q, t) = 0
(6.6.3)
é ν (q, t) ù
C(q, t)q& = ê h ú º ν(q, t)
ë e(q, t) û
é -g (q, q& , t) ù
q=ê h
C(q, t)&& ú º - γ (q, q& , t)
ë - γ nh (q, q& , t) û
where q 0 and q& 0 satisfy the fourth and fifth equations at t 0 .
545
Theorem 6.6.4 Well Posed Nonholonomic Full DAE If
(1) all functions appearing in Eqs. (6.6.3) are k ³ 1 times continuously differentiable on a
bounded domain D of q-q& -t-b space, where b is a vector of parameters appearing in the
functions,
(2) the matrix C(q , t) has full rank on D,
(3) the mass matrix M (q ) is positive definite on the null space of C(q , t) ; i.e.,
qT M(q)q > 0 for all q ¹ 0 such that C(q, t)q = 0 , and
(4) q 0 and q& 0 satisfy the fourth and fifth of Eqs.(6.6.3) at t 0 ,
then Eqs. (6.6.3) have a unique solution q (t, b ) in a neighborhood of (q 0 , q& 0 , t 0 , b)

that is k times continuously differentiable with respect to t and b.
Derivation of the Full DAE initial-value problem shows that a solution of the variational
formulation is a solution of the Full DAE formulation. Since the variational and ODE
formulations are equivalent, a solution of the ODE formulation generates, via Eqs. (6.1.22),
(6.1.35), and (6.1.39), a solution of the Full DAE formulation. To show the converse is true,
using the constraint parameterizations of Section 6.1 and suppressing arguments, the first of Eqs.
(6.6.3) becomes
& + CT λ - MXHγ - S - Q A = 0
MKw (6.6.4)
Multiplying by X T ,
& + X T CT λ - X T ( MXHγ + S + Q A ) = 0
X T MKw
The transpose of Eq. (6.1.29) yields ( H -1 ) = X TCT , so ( XTCT )-1 = H T . Thus,

T
& + H T X T ( MXHγ + S + Q A )
λ = - H T X T MKw
(I - C H
T T
X T ) MKw
& - ( I - CT H T X T )( MXHγ + S + Q A ) = 0
Multiplying this relation on the left by WT and using the first of Eqs. (6.1.25), this is
& - K T ( MXHγ + S + Q A ) = 0
K T MKw (6.6.5)
Taken with the kinematic differential equation of Eq. (6.1.40), this is the first order
ODE of Eqs. (6.4.6). Since the initial conditions of Eqs. (6.6.3) imply those of Eq. (6.6.2), a
solution of the Full DAE initial-value problem is a solution of the ODE initial-value problem.
To establish equivalence of the two formulations, it remains only to show that a solution of
the Full DAE initial-value problem is unique. First, recall that the Lagrange multiplier
theorem implies that λ is unique. To obtain a contradiction that shows q is unique, assume
546
that two solutions q and q of the Full DAE initial-value problem are distinct; i.e., q ¹ q .
They generate solutions v and v of the ODE initial-value problem that satisfy Eq. (6.1.22),
so q - q = V ( v - v ) - U ( h( v, b,t) - h( v , b,t) ) . Since the solution of the ODE initial-value
problem is unique, v = v and q - q = 0 . This establishes the desired contradiction and
completes the proof.
Results presented show that four nonholonomic system formulations are well posed, as in the
case of holonomic systems.
547
Appendix 6.A Tangent Space Nonholonomic Code
Code 6.3.1 Three Wheel Transporter-TSODE

Code 6.3.2 Ellipsoid Rolling on Moving Surface-TSODE
Code 6.5.1 Planar Articulated Vehicle-TSIndex 0
Code 6.5.2 Disk Rolling on x-y Plane-TSIndex 0
548
549
CHAPTER 7
DAE Methods for Holonomic Systems
7.0 Introduction
The differential-algebraic equations (DAE) of multibody dynamics that are introduced in

Section 4.10 are shown in Section 5.11 to be equivalent to tangent space ODE of Chapter 5,
hence to be well posed. Existence, uniqueness, and regularity of solutions as functions of
problem data, or design, are thus established. Construction of solutions, however, remains a
difficult task. The concept of index of a DAE is introduced in Section 7.1 and a landmark paper
(Petzold, 1982), DAE are not ODE, is cited as the basis for the Petzold Imperative that serves as
a caution regarding unjustified direct application of ODE formulas for solution of the DAE of
multibody dynamics.
Three formulations of DAE of multibody dynamics, based on the index of equations that
enforce only one of the three forms of kinematic constraint, are presented in Section 7.2, with
methods for correcting error that accumulates in ignored constraints. Numerical approaches for
solution of Index 1, 2, and 3 DAE are outlined in Section 7.3, including implementation of
numerical integration methods.
A rudimentary general-purpose MATLAB Code 7.4 is presented in Section 7.4 that
implements the methods of Sections 7.1 through 7.3 for simulation of planar multibody systems.
Code 7.4 is used in Section 7.5 to simulate two planar systems that have been treated in
preceding chapters and to study properties of the three DAE formulations and numerical
integration algorithms for their solution. This process is repeated in Sections 7.6 and 7.7 for
spatial systems. Results suggest that both explicit and implicit integration methods applied to
Index 1 and 2 formulations yield reasonable results. Direct application of even implicit
integrators to the Index 3 formulation, however, leads to marginal results or outright failure.
Codes 7.4 and 7.6 for simulation of planar and spatial multibody systems, respectively, are
contained in Appendix 7.A.
550
7.1 The DAE of Holonomic Systems
A brief, but rigorous, derivation of the Lagrange multiplier form of the equations of
multibody dynamics is presented, followed by definition of the differentiation index, or simply
index. An alternative formulation with the Lagrange multiplier as the derivative of an unknown
quantity is presented, yielding an index one lower than the classic formulation. Finally,
fundamental results published by Petzold (1982) are discussed, providing an “imperative” that
DAE are not ODE and suggesting that care be taken in using ODE methods for solution of DAE.
7.1.1 Lagrange Multipliers
Configurations of holonomic systems are characterized by ngc generalized coordinates
T
q = éëq1T L q nb
T
ùû Î R ngc (7.1.1)
that satisfy nhc < ngc holonomic constraints

F ( q , t) = 0 (7.1.2)
and their first and second derivatives
F q (q, t)q& = -F t (q, t)
( )
(7.1.3)
F q (q, t)q
&& = - F q (q, t)q& q& - 2F tq (q, t)q& - F tt (q, t) º - γ
q
d’Alembert (1743) and Lagrange (1788) provided the foundation for dynamics of
holonomic systems, in the form of the variational equation of motion for systems of particles,
called the “fundamental equation” by Pars (1965). The extension presented in Chapter 4 to
account for constrained rigid bodies is
δ q T ( M ( q )q
&& - S(q, q& ) - Q A (q, q& , t) ) = 0 (7.1.4)
for all virtual displacements δq such that

Fq (q, t)δq = 0 (7.1.5)
A crucial aspect of this formulation is that the virtual work of constraint reaction forces is zero,
for all δq that satisfy Eq. (7.1.5). Thus, constraint reaction forces do not appear in Eq. (7.1.4).
At this point in the classical literature (Lanczos, 1962), a heuristic (nonrigorous)
argument is made that one can multiply Eq. (7.1.5) by a vector l T to obtain l TFq δq = 0 and add
this form of zero to Eq. (7.1.4), obtaining δq T ( M( q)q
&& - S(q, q& ) - Q A (q, q& , t) ) + l TFq (q, t) = 0 , or
δqT ( M( q )q
&& + FqT (q,t)l - S(q, q& ) - Q A (q, q& ,t) ) = 0 (7.1.6)
The folklore continues with the heuristic argument that l may be selected so that the coefficient
of δqT in Eq. (7.1.6) is zero; i.e.,
&& + FqT (q, t)l - S(q, q& ) - Q A (q, q& , t) = 0
M(q )q (7.1.7)
551
This heuristic argument can be corrected by invoking the Lagrange Multiplier Theorem
of Section 2.2.2, as follows: provided Fq has full rank, if Eq. (7.1.4) holds for all δq that satisfy
Eq. (7.1.5), there exists a unique vector l such that Eq. (7.1.6) holds for arbitrary δq , hence
(7.1.7) is valid. The result is the same equation, but the latter argument rigorously assures
existence of a unique Lagrange multiplier, whereas the former does not. In fact, the heuristic
argument says nothing about the rank of Fq .
Finally, in order to have any chance for a unique solution of Eq. (7.1.7), initial conditions
q(t 0 ) = q0
(7.1.8)
q& (t 0 ) = q& 0
at time t 0 must be defined that satisfy Eq. (7.1.2) and the first of Eqs. (7.1.3); i.e.,
F(q 0 , t 0 ) = 0
(7.1.9)
Fq (q0 , t 0 )q& 0 = -F t (q 0 , t 0 )
Equations (7.1.7) and (7.1.8) are called differential-algebraic equations (DAE), since λ
appears algebraically and the algebraic constrains of Eq. (7.1.2) must be satisfied. As shown in
Chapter 5, under reasonable hypotheses these equations have a unique solution that satisfies all
three forms of constraint in Eqs. (7.1.2) and (7.1.3). That theory, however, says nothing about
how to find the unique solution.
With the foregoing one page rigorous derivation, the DAE formulation is easily obtained;
i.e., it is “a cheap result”. The moral of this chapter, however, is that “you get what you pay for”.
In short, the foregoing DAE are extraordinarily difficult to solve. One of the complexities
encountered is that, in addition to the constraint of Eq. (7.1.2), its first and second derivatives
must be satisfied; i.e., the kinematic velocity and acceleration equations of Eqs. (7.1.3).
Equations (7.1.3) have, unfortunately, inherited the name hidden constraints in the multibody
dynamics literature. It is critical that they not be hidden, or more accurately ignored, in the
theory and numerical methods of multibody dynamics.
7.1.2 Index of DAE
The differentiation index of a DAE is defined (Ascher and Petzold, 1998; Hairer and
Wanner, 1996) as the minimum number of differentiations of equations in the DAE required to
reduce it to an ODE in the solution variables. While there are other definitions of index, the
differentiation index has become the most commonly used as a measure of the difficulty in
obtaining numerical solutions. For the holonomic DAE of Eqs. (7.1.2) and (7.1.7), the constraint
equation may be differentiated twice to obtain the constraints of Eq. (7.1.3). Combining Eq.
(7.1.7) with the second of Eqs. (7.1.3), in matrix form, yields
é M(q) F q (q, t )T ù é&&q ù éQ A (q,q,
& t ) + S(q,&q) ù
ê úê ú = ê ú (7.1.10)
ëêF q (q, t ) 0 ûú ë λ û ë -γ û
Under the conditions that the constraint Jacobian F q (q, t) has full row rank and M (q ) is
positive definite on the null space of F q (q, t) , it is to be shown that the coefficient matrix on the
552
left of Eq. (7.1.10) is nonsingular. Suppressing arguments of functions for notational
convenience, consider the equation
éM F qT ù éα ù
ê úê ú =0 (7.1.11)
êë F q 0 úû ë β û
If the only solution of Eq. (7.1.11) is α = 0 and β = 0 , the coefficient matrix is nonsingular.
Multiplying the first row of Eq. (7.1.11) by α T ,
α T Mα + α T F qT β = α T Mα + β T F q α = 0
Since the second row of Eq. (7.1.11) is F qα = 0 , this reduces to α T Mα = 0 . As shown in Section
4.6.2, M is positive definite on the null space of the constraint Jacobian, so α = 0 . The first row
of Eq. (7.1.11) reduces to F Tq β = 0 and, since F qT has full column rank, β = 0 . Thus, the
coefficient matrix of Eq. (7.1.10) is nonsingular.
The solution of Eq. (7.1.10) is
-1
é&&q ù é M(q) F q (q, t ) ù éQ A (q,q, & t) ù
& t ) + S(q,&q) ù é f (q,q,
T
êλ ú = ê ú ê úºê & t ) úû
(7.1.12)
ë û êëF q (q, t ) 0 úû ë -γ û ëg(q,q,
Differentiating the second row yields the system of equations
&& = f
q
(7.1.13)
λ& = g qq& + g q q
&& + g t = g qq& + g qf + g t
which is an ODE in the solution variables q and λ . Since three derivatives were required, two of
Eq. (7.1.2) to obtain the second of Eqs. (7.1.3) and one of the second row of Eq. (7.1.12), the
DAE of Eqs. (7.1.2) and (7.1.7) is of Index 3.
7.1.3 An Alternative DAE Formulation
In the holonomic DAE of Eqs. (7.1.2) and (7.1.7), the variable λ (t) may be integrated to
define
t
μ (t) º ò λ (t)dt
t0
(7.1.14)
μ (t 0 ) = 0
Differentiating yields
μ& = λ (7.1.15)
The DAE of Eq. (7.1.7) may thus be written as
&& + Fq (q, t)T&μ - S(q,&q, t ) - Q A (q,q,
M(q)q & t) = 0 (7.1.16)
with holonomic constraints of Eq. (7.1.2). Combining the second of Eqs. (7.1.3), which required
two differentiations of Eq. (7.1.2), with Eq. (7.1.16) yields the ODE
553
é M(q) FTq (q, t) ù éq
&& ù é S(q, q& , t) + Q A (q, q& , t) ù
ê T ú =ê ú
0 úû êëμ& úû ë
(7.1.17)
êë Fq (q, t) - γ (q, q& , t) û
Thus, the DAE of Eqs. (7.1.16) and (7.1.2) is of Index 2.
This result is perplexing. The DAE of Eqs. (7.1.7) and (7.1.2) is clearly equivalent to that
of Eqs. (7.1.16) and (7.1.2), with only the name of the multiplier changed. Yet one is of Index 3
and the other is of Index 2. It is inconceivable that a mere renaming of variable can change a
meaningful problem characteristic, suggesting that the differentiation index may be of dubious
value.
7.1.4 The Petzold Imperative; DAE are not ODE
In 1982, Petzold published a landmark paper entitled Differential/Algebraic Equations
are not ODE’s (Petzold, 1982); i.e., DAE are not ODE. The focus was on behavior of numerical
integration methods that were being considered for solution of DAE at the time, which was the
infancy of research on methods of solving DAE. Among other things, it was shown that direct
application of ODE integration methods in an attempt to numerically solve DAE, especially
those of Index 3, is problematic and potentially disastrous. This author has taken the Petzold
Imperative seriously, applying ODE integration formulas to only ODE; hence the origin of the
initial focus of this text on formulations that create ODE of multibody dynamics and their
associated numerical solution using ODE integrators.
As observed in previous subsections, the Petzold Imperative is not limited to numerical
integration issues. The unfortunate concept of hidden constraints reflects a viewpoint that “if it
were not for these pesky hidden imperfections, the DAE of multibody dynamics would be ODE”.
As seen in Chapter 5, the tangent space formulation yields ODE that may be integrated with
numerical error control on all three forms of constraint. Numerical approaches to direct solution
of the DAE of multibody dynamics using ODE integrators that have evolved in a massive
research effort over the last third of a century are considered in the remainder of this chapter.
Most, if not all, methods proposed and currently used in numerical solution of the DAE of
mechanical system dynamics are based on applying or adapting ODE numerical formulas for
solution of the DAE, without a near perfect solution in sight.
The concept of index of a DAE provides a basis for making distinctions between alternative
formulations of the equations of multibody dynamics. Potential difficulties that are
encountered in the Index 3 formulation identified by Petzold serve as a warning that severe
problems may be encountered in direct integration of the Index 3 DAE of multibody dynamics
using ODE methods.
554
7.2 Formulations for Numerical Solution of DAE
While it is shown in Section 5.11 that the Full DAE of properly modeled holonomic
multibody dynamic systems have unique solutions that depend continuously on time and model
parameters, there are severe difficulties in constructing these solutions. The Petzold Imperative
of Section 7.1.4 emphasizes that DAE are not ODE, suggesting that direct application of ODE
integrators to generalized coordinates in the DAE setting may lead to erroneous results. An
extensive literature, far too large in scope to summarize here, confirms the risk in direct
application of ODE methods to Index 3 DAE; e.g., (Brenan, Campbell, and Petzold, 1989;
Ascher and Petzold, 1998; Hairer and Wanner, 1996; Rabier and Rheinboldt, 2002; Arnold,
2017).
In spite of the foregoing warnings, hundreds of papers have been published that attempt
to integrate DAE through application of ODE integrators, some based on modifications of ODE
integrators to correct errors introduced. Approaches presented include modifying ODE
predictions to satisfy constraints that are unenforced in the formulation, or adding penalty terms
or additional multiplier terms in the equations of motion in an attempt to compensate for errors
introduced by direct application of ODE integrators to the DAE. This might be thought of as the
Larry Bird Paradox, in honor of the legendary basketball player who is reported to have declared
that he made a career in basketball “compensating for a lack of speed”. The analogy in
multibody dynamics is making a career of compensating for application of ODE methods for
solution of DAE, contrary to the Petzold Imperative.
Some numerical approaches in the literature have been based on reduction of the DAE of
motion to equivalent ODE (Wehage and Haug, 1982; Potra and Rheinboldt, 1991; Negrut, Haug,
and German, 2003), called the state space approach. The tangent space ODE formulation of
Chapter 5 is in this class. While accepted as valid methods of solution, such approaches have
often been dismissed as too complicated and computationally intensive. Proponents of the state
space ODE approach have observed that “you get what you pay for” and have suggested that the
only good DAE is an equivalent ODE. Without further focus on this debate, the remainder of this
chapter presents three approaches to adaptation of ODE methods for numerical solution of the
DAE of multibody dynamics and their implementation in MATLAB computer code for
numerical test and evaluation.
To be specific regarding formulations that use ODE integrators for solution of the DAE
of multibody dynamics, three forms of holonomic DAE are considered. Since almost no attention
has been given to numerical methods for nonholonomic DAE, other than that of Chapter 6,
consideration here is limited to holonomic systems.
The Index 1 formulation of the DAE of Eqs. (7.1.2) through (7.1.8) is to enforce the
equations of motion, acceleration constraint equations, and initial conditions; i.e.,
&& + F qT (q, t)l = Q A (q, q& , t) + S(q, q& )
M(q )q
q = - ( F q (q, t)q& ) q& - 2F tq (q, t)q& - F tt (q, t) º -g (q, q& , t)
F q (q, t)&& (7.2.1)
q
q(t 0 ) = q0 ; q& (t 0 ) = q& 0

but to leave the configuration and velocity constraint equations
555
F (q, t) = 0
(7.2.2)
F q (q, t)q& = -F t (q, t)
unenforced, except to require that initial values q 0 and q& 0 of Eq. (7.2.1) satisfy Eqs. (7.2.2).
equations of motion, velocity constraint equations, and initial conditions; i.e.,
q + F qT (q, t)l = Q A (q, q& , t) + S(q , q& )
M(q )&&
F q ( q, t)q& = -F t (q, t) (7.2.3)
q(t 0 ) = q0 ; q& (t 0 ) = q& 0
but to leave the configuration and acceleration constraint equations
F (q, t) = 0
(7.2.4)
F q (q, t)q
&& = - γ(q, q& , t)
unenforced, except to require that the initial values q 0 and q& 0 of Eq. (7.2.3) satisfy Eqs. (7.2.2)
&&0 and l 0 satisfy the first two of Eqs. (7.2.1).
and initial values q
equations of motion, configuration constraint equations, and initial conditions; i.e.,
&& + F qT (q, t)l = Q A (q, q& , t) + S(q, q& )
M(q )q
F(q, t) = 0 (7.2.5)
q(t 0 ) = q0 ; q& (t 0 ) = q& 0
but to leave the velocity and acceleration constraint equations
F q (q, t)q& = -F t (q, t)
(7.2.6)
F q (q, t)q
&& = -g (q, q& , t)
unenforced, except to require that the initial values q 0 and q& 0 of Eq. (7.2.5) satisfy Eqs. (7.2.2)
&&0 and l 0 satisfy the first two of Eqs. (7.2.1).
and q
7.2.4 Correction of Constraint Errors
If errors in velocity or configuration constraints that are unenforced in one of the DAE
formulations exceed prescribed tolerances, perturbations in the associated variables are often
determined as corrections that prevent the approximate solution from becoming physically
meaningless.
If the velocity constraint is unenforced, a perturbation Dq& of minimum norm is desired
such that F q (q& + Dq& ) + F t = 0 ; i.e.,
556
minimize ( Dq& T Dq& / 2 ) , such that F q (q& + Dq& ) + F t = 0 (7.2.7)
The necessary condition for a solution is existence of a multiplier vector m Î R nhc such that
( Dq& Dq& / 2 + m
T T
(Fq (q& + Dq& ) + F t ) ) Dq&
= Dq& T + m T Fq = 0
(7.2.8)
Fq (q& + Dq& ) + F t = 0
with the solution
Dq& = -F Tq m (7.2.9)
where m is determined by the condition F q (q& - F qT m ) + F t = 0 , or
F q F Tq m = F q q& + F t (7.2.10)
If the configuration constraint is unenforced, a perturbation Dq of minimum norm is

desired such that F(q + Dq) = 0 ; i.e.,
minimize ( Dq T Dq / 2 ) , such that F (q + Dq) = 0 (7.2.11)
The necessary condition for a solution is existence of a multiplier vector τ Î R nhc such that
( D q D q / 2 + τ F (q + Dq ) )
T T
Dq
= D q T + τ TF q ( q + D q ) = 0
(7.2.12)
F( q + D q ) = 0
Since these are nonlinear equations in τ and Dq , they must be solved using an iterative
approach, such as the Newton-Raphson method. With an initial estimate Dq1 = 0 and τ1 = 0 , the
iterative solution equation is
é I ngc + P4(q + Dqi , τ i ) F q (q + Dqi )T ù é Dqi ù é Dq i + F Tq (q + Dqi )τ i ù
ê údê i ú = -ê ú
ë F q ( q + Dq i ) 0 û ë τ û ë F(q + Dq i ) û
(7.2.13)
é Dq i+1 ù é Dq i ù é Dq i ù
ê i +1 ú = ê i ú + δ ê i ú
ë τ û ë τ û ë τ û
i=0,1,..., until F (q + Dq i ) + D qi + F qT (q + Dqi ) τi £ i ntol .
The results of these calculations yield perturbations to approximate solutions that satisfy
velocity and/or configuration constraints that have been ignored, to within specified tolerances.
The correction process, however, contributes errors to the approximate solution, beyond those
already present in the numerical integration approximation process. One might be suspicious that
the Petzold Imperative will dominate and poor approximations will result; i.e., the Larry Bird
Paradox. While there is some theory in the literature to justify this correction approach in
selected applications, numerical experiments are required to assess the validity of these
approximations.
557
7.2.5 Numerical Approaches to Solution of DAE
In Section 7.2.1, the DAE of Eqs. (7.2.1) reduces to an ODE with one differentiation,
hence it is of Index 1. Since one differentiation of the velocity constraint of the DAE of Eqs.
(7.2.3) reduces to the DAE of Eqs. (7.2.1), the DAE of Eqs. (7.2.3) is of Index 2. Finally, the
analysis of Section 7.1.2 has shown that the DAE of Eqs. (7.2.5) is of Index 3.
As shown in Section 7.1.3, the Index 1 form of the equations of motion of Eqs. (7.2.1)
&&n , which may be integrated using an explicit numerical integration
can be solved at time t n for q
formula, such as RKFN45 or Nystrom4 of Section 4.8. This constitutes an explicit numerical
integration approach for the Index 1 formulation. It says nothing, however, about the accuracy
with which the unenforced position and velocity constraint equations of Eq. (7.2.2) are satisfied.
The Index 2 and 3 formulations of Eqs. (7.2.3) and (7.2.5) do not yield to this explicit integration
approach.
If an implicit ODE integration formula is applied to the acceleration variable q && and the
result is substituted into Eqs. (7.2.1), (7.2.3), or (7.2.5), a system of ngc + nhc nonlinear
equations in the ngc + nhc variables q &&n and λ n is obtained at integration time step t n . There is
thus the prospect, but not assurance, that these equations can be solved for an approximate
solution. There is no guarantee, however, that unenforced constraint equations of Eqs. (7.2.2),
(7.2.4), or (7.2.6) are even approximately satisfied. This is one consequence of the Petzold
Imperative that DAE are not ODE; i.e., that application of an ODE integrator to something that is
not an ODE is not guaranteed to provide meaningful results.
Theoretical results have been presented showing that the direct approach to solution using
the Index 1 formulation yields reasonable solutions (Brenan, Campbell, and Petzold, 1989;
Hairer and Wanner, 1996; Arnold, 2017) and that slightly less assurance is offered for solutions
obtained using the Index 2 formulation. Severe warnings are provided that results obtained using
the Index 3 formulation may be unreliable. Results obtained using the Index 3 formulation and
backward differentiation ODE integration formulas (Ascher and Petzold, 1998; Hairer and
Wanner, 1996) in the landmark paper (Orlandea, Chace, and Calahan, 1977) has shown over
time that, with great care, the direct approach using the Index 3 formulation can yield acceptable
results. A more recent contribution (Negrut, Rampalli, Ottarsson, and Sajdak, 2007) presented a
DAE integration formulation with the HHT formulation for structural dynamics (Hilber, Hughes,
and Taylor, 1977) that has been shown to yield reasonable results for the Index 3 formulation of
the equations of multibody dynamics (Arnold and Bruls, 2007).
In the HHT formulation (Negrut, Rampalli, Ottarsson, and Sajdak, 2007), the discretized
equations of motion of Eq. (7.2.5) are modified by introducing a parameter α , -1/ 3 £ a £ 0 , at
time t n into the discrete form of the equations of motion,
(1 / (1 + α) ) M (q n )q&& n + F qT (q n , t n )l n - Q A (q n , q& n , t n ) - S(q n , q& n )

(7.2.14)
- ( α / (1 + α) ) ( F qT (q n -1 , t n -1 )l n -1 - Q A (q n -1 , q& n -1 , t n -1 ) - S(q n -1 , q& n -1 ) ) = 0
The trapezoidal numerical integration algorithm of Eq. (4.8.40) is similarly modified as
558
q n = q n -1 + hq& n -1 + (h 2 / 2) ( (1 - 2β)&& && n )
q n -1 + 2βq
(7.2.15)
q& n = q& n -1 + h ( (1 - δ)q &&n )
&&n -1 + δq
where
β = (1 - α) 2 / 4
(7.2.16)
δ = (1 - 2α) / 2
In the literature, the parameter δ used here is called γ . The modification in terminology used
here is to avoid conflict with the function g ( q, q& ) that appears in the kinematic acceleration
equations.
It is interesting that the HHT method uses an integration formula of Eq. (7.2.15), which is
not technically valid as an ODE integrator, with Eq. (7.2.14) which is not a discretized form of
the multibody system equation of motion. The result is a method that incorporates numerical
damping to avoid divergence of the method when it is applied to the multibody system equations
of motion (Negrut, Rampalli, Ottarsson, and Sajdak, 2007; Arnold and Bruls, 2007). It is also
interesting that the HHT method reduces to the trapezoidal method when α = 0 . A recent paper
(Schweizer and Pu, 2016) presents a similar approach that modifies Runge-Kutta ODE
integration stage equations to enable satisfaction of all three forms of kinematic constraint, at the
expense of failing to be a valid ODE integration formula.
The explicit Nystrom4 and RKFN45 integrators outlined above for the Index 1
formulation and the implicit trapezoidal and HHT integrators for Index 1, 2, and 3 formulations
are implemented in the following sections, to enable systematic study of properties of each
method.
7.2.6 Third Derivative for Error Control
For the case in which there is no explicit time dependence in the kinematic constraints
and generalized forces, the Index 1 formulation of equations of motion of Eq. (7.2.1) may be
differentiated with respect to time, to obtain
M(q )&&& (
q + F Tq (q,t)l& = - M (q )q ) (
&&ˆ q& - F q (q, t)T lˆ
q
) q
q& + QqA (q, q& )q& + Sq (q, q& )q&
+ QqA& (q, q& )&&

q + Sq& (q, q& )&&
q (7.2.17)
F q (q, t)&&& (
q = - F q (q, t)q )
&&ˆ q& - g qq& - g q& q
q
&&
Using the derivative operator notation of Section 5.3 and suppressing arguments of functions, in
matrix form, this is
éM F Tq ù é&&&
ê
(
q ù é -M 2(q, q) && ù
&& - P4(q, λ) + S q + QqA q& + Sq& + QqA& q
ú
) ( )
ê úê ú = (7.2.18)
ëê F q 0 ûú ë λ& û ê
ë ( -P2(q,q) - g q ) q - g q& q
&& & && ú
û
The third derivative of generalized coordinates computed using Eq. (7.2.18) can be used in error
estimates, as in Section 4.8.4. They and the first derivatives of Lagrange multipliers can also be
used in obtaining estimates to start iterative solution of discretized equations of motion.
559
Index 1, 2, and 3 formulations of holonomic DAE enforce only one of the three forms of
kinematic constraint. Errors in unenforced constraints may be corrected, but at the cost of
introducing further error in the approximate solution. Numerical approaches for approximate
solutions presented in subsequent sections use error estimates based on third derivatives of
solutions for variable step size error control.
Key Formulas
Index 1:
q + F Tq (q, t)l = Q A (q, q& , t) + S(q, q& )
M(q )&&
&& = - ( F q (q, t)q& ) q& - 2F tq (q, t)q& - F tt (q, t) º - γ(q, q& ,t)
F q (q, t)q (7.2.1)
q
q(t 0 ) = q0 ; q& (t 0 ) = q& 0
Index 2:
q + F Tq (q,t)l = Q A (q, q& ,t) + S(q, q& )
M(q)&&
F q (q, t)q& = -F t (q, t) (7.2.3)
q(t ) = q ; q& (t ) = q&
0 0 0 0
Index 3:
&& + F Tq (q,t)l = Q A (q, q& ,t) + S(q, q& )
M(q)q
F (q, t) = 0 (7.2.5)
q(t ) = q ; q& (t ) = q&
0 0 0 0
560
7.3 Numerical Integration of Index 1, 2, and 3 DAE of Holonomic Systems
The Index 1 DAE is integrated using either explicit integration methods or the implicit
HHT method, whereas the Index 2 and 3 DAE may only be treated using the implicit approach.
7.3.1 Integration of Index 1 DAE
Numerical integration of the Index 1 formulation of the DAE for holonomic systems of
Section 7.2.1 can be implemented with explicit and implicit numerical integration algorithms.
The integration algorithms presented are implemented for both planar and spatial systems, using
adaptations of MATLAB Codes 5.7 and 5.9 that are presented in Sections 5.7 and 5.9.
7.3.1.1 Explicit Integration of Index 1 DAE
The first two equations of Eqs. 7.2.1, in matrix form, are
é M(q) F qT (q, t ) ù é&&q ù éQ A (q,q, & ù
& t ) + S(q,q)
ê úê ú = ê ú (7.3.1)
ëêF q (q, t ) 0 ûú ë λ û ë & ,t )
- γ(q,q, û
Beginning at t 0 with initial values q(t 0 ) = q 0 and q& (t 0 ) = q& 0 of Eqs. (7.2.1), Eq. (7.3.1) is
&&0 and λ 0 , which are stored as solution variables. An explicit numerical integrator,
solved for q
such as Nystrom 4 or RKFN45, is used to integrate for q1 and q& 1 , the approximate solution at
t1 = t 0 + h . The process is repeated to solve Eq. (7.3.1), evaluated at q1 and q& 1 to obtain
q&&1 and λ1 , which are stored as solution variables. At time t n with qn and q& n known, Eq. (7.3.1)
is evaluated at q n and q& n and solved for q&&n and λ n , which are integrated for qn +1 and q& n +1 and
are stored as solution variables. The process is repeated until the final time is reached or a
singularity is encountered.
7.3.1.2 Implicit Integration of Index 1 DAE
Section 7.2.1 may be implemented with implicit numerical integration formulas based on the
HHT method of Section 7.2.5. The formulation is applicable for both planar and spatial systems,
using the foundations of MATLAB Codes 5.7 and 5.9.
The Index 1 equations of motion of Eq. (7.2.1) are written in residual form as
é R1 ù é M(q )q
&& + F Tq (q, t)l - Q A (q, q& , t) - S(q, q& ) ù
Rºê ú=ê ú=0 (7.3.2)
ë R 2û ë F q (q, t)q&& + γ (q, q& ,t) û
For the HHT implicit numerical integration approach of Section 7.2.5, these equations are
modified and written at time t n in the form
R1 = (1 / (1 + α))M(q n )&&
q n + F Tq (q n , t n )l n - Q A (q n , q& n , t n ) - S(q n , q& n )
- (α / (1 + α)) ( F qT (q n -1 , t n -1 )l n -1 - Q A (q n -1 , q& n -1 , t n -1 ) - S(q n -1 , q& n -1 ) ) (7.3.3)
R 2 = F q (q n , t n )q
&& n + γ(q n , q& n )
It should be noted that this reduces to direct application of the trapezoidal method applied to Eq.
(7.3.2) when a = 0 .
561
To evaluate the Jacobian of these equations, using the derivative operators of Chapter 5,
R1&&q n = (1 / (1 + α))M(q n )
R1q& n = - QqA& n (q n , q& n , t n ) - Sq& n (q n , q& n )
&& n )q + ( F Tq (q n , t n )l n ) - QqAn (q n , q& n , t n ) - Sq n (q n , q& n )
R1q n = (1 / (1 + α)) ( M (q n )q
n qn
= (1 / (1 + α))M 2(q n , &&

q n ) + P4(q n , l n ) - Q (q n , q& n , t n ) - Sq n (q n , q& n )
A
qn
R1λ n = F qT (q n , t n )
(7.3.4)
R 2&&q n = F q (q n , t n )
R 2q& n = γ q& n (q n , q& n , t n )
R 2q n = ( F q (q n ,t n )q
&& n ) + γ q n (q n , q& n ,t n )
qn
= P2(q n , q
&&n ) + γ q n (q n , q& n ,t n )
R 2λ n = 0
Using the HHT integration formulas of Eq. (7.2.15) and the chain rule of differentiation,
dR1
d&&
qn
(
= (1/ (1 + α))M (q n ) - hδ QqA& n (q n , q& n , t n ) + Sq& n (q n , q& n ) )
(
+ h 2β (1 / (1 + α))M 2(q n , q )
&&n ) + P 4(q n , l n ) - QqAn (q n , q& n , t n ) - Sqn (q n , q& n ) (7.3.5)
dR 2
&&n
dq
(
= F q (q n , t n ) + hδγ q& n (q n , q& n , t n ) + h 2β P 2(q n , q
&&n ) + γ qn (q n , q& n ,t n ) )
The Jacobian of the residual is thus
é dR1 ù
ê d&& F Tq (q n , t n ) ú
qn
J=ê ú (7.3.6)
ê dR 2 ú
ê d&& 0 ú
ë qn û
&&0 and λ 0 . At
Beginning with initial conditions of Eq. (7.2.1), Eq. (7.3.1) is solved for q
time step t n , n = 1,L, n , the solution variables q
&&n and λ n are estimated as q &&1n = q
&&n -1 and
λ1 = λ , or q
n n -1
&&1 = &&
n q1 and λ1 = λ1 + hλ& 1 if derivatives &&&
q1 + h&&& n -1 n n n -1 q1 and λ& 1 are computed, as in
n n n
Section 7.2.6. Generalized coordinates q and q& are evaluated using Eq. (7.2.15). The Jacobian
1
n
1
n
of Eq. (7.3.6) and residual of Eq. (7.3.3) are evaluated as J1 and R1 . The first step in Newton -
Raphson iteration is carried out by solving
J1x = - R1 (7.3.7)
With Pq&& º éë I ngc 0ùû and Pλ º ëé0 I nc ùû , Dq
&&1n = P&&q x and Dλ1n = Pλ x . The values of solution
variables are thus updated as
562
&& 2n = q
q &&1n + D q
&&1n
(7.3.8)
λ 2n = λ1n + D λ1n
With the Jacobian held fixed at J = J1 , the process is continued until R i £ intol , the integration
tolerance, to determine q n , q& n , q
&& n , and λ n . The process is repeated at t n +1 and continued until
the final time is reached, or a singularity is encountered.
The trapezoidal integration algorithm is obtained by setting a = 0 in the preceding
development.
Section 7.2.2 is implemented with implicit numerical integration formulas based on the HHT
method. The formulation is applicable for both planar and spatial systems, using the foundations
of MATLAB Codes 5.7 and 5.9.
é R1 ù é M(q )q
&& + F Tq (q, t)l - Q A (q, q& , t) - S( q, q& ) ù
Rºê ú=ê ú=0 (7.3.9)
ë R 2 û ëê (1 / (δh)) ( F q (q,t)q& + F t (q, t) ) ûú
The factor 1/ (δh) , where d is given in Eq. (7.2.16), is introduced to enhance the condition of
the Jacobian of the discretized equations. For HHT implicit integration, these equations are
modified and written at time t n in the form of Eqs. (7.3.3), where R1 is given in Eq. (7.3.3), and
(
R 2 = (1/ (δh)) F q (q n , t n )q& n + F t (q n , t n ) ) (7.3.10)
To evaluate the Jacobian of Eq. (7.3.9), derivative operators of Section 5.3 are used for
R1 and
R 2&&qn = 0
R 2q& n = (1/ (δh))F q (q n , t n )
((
R 2qn = (1/ (δh)) F q (q n , t n )q& n ) qn
+ F tq (q n , t n ) ) (7.3.11)
(
= (1/ (δh)) P 2(q n , q& n ) + F tq (q n , t n ) )
R 2λ n = 0
dR1
Using the HHT integration formulas of Eq. (7.2.15) and the chain rule of differentiation, is
&& n
dq
as given in Eq. (7.3.5) and
dR 2
&&n
dq
(
= F q (q n , t n ) + (hβ / δ) P 2(q n , q& n ) + F tq (q n , t n ) ) (7.3.12)
The Jacobian of the residual of Eq. (7.3.9) is thus
563
é dR1 ù
q
J=ê n ú (7.3.13)
ê dR 2 ú
ê d&& 0 ú
ë qn û
Beginning with initial conditions of Eq. (7.2.1), Eq. (7.3.1) is solved for q &&0 and λ 0 . At
&&n and λ n are estimated as q &&1n = q&&n -1 and
λ1 = λ , or q
n n -1
&&1 = &&
q1 + h&&&
n -1 n n n -1 n q1 and λ& 1 are computed, as in
n n
1
n
1
n
of Eq. (7.3.13) and residual of Eq. (7.3.10) are evaluated as J1 and R1 . The first step in Newton-
J1x = - R1 (7.3.14)
With Pq&& º éë I ngc 0ùû and Pλ º éë0 I nc ùû , Dq
&&1n = P&&q x and Dλ1n = Pλ x . The values of solution
variables are then updated as
&& 2n = q
q &&1n + D q
&&1n
(7.3.15)
λ 2n = λ1n + D λ1n
&& n , and λ n . The process is repeated at t n +1 and continued until
Section 7.2.3 is implemented with implicit numerical integration formulas based on the HHT
method. The formulation is applicable for both planar and spatial systems, using the foundations
of MATLAB Codes 5.7 and 5.9.
é R1 ù é M( q )&&
q + F Tq (q , t)l - Q A (q , q& , t) - S( q, q& ) ù
Rºê ú=ê ú=0 (7.3.16)
ë R 2û ë (1 / βh 2 )F( q, t) û
The factor 1/ (βh 2 ) is introduced to enhance the condition of the Jacobian of the discretized
equations. For HHT implicit numerical integration, these equations are modified and written at
time t n in the form of Eq. (7.3.3), where R1 is given in Eq. (7.3.3), and
R 2 = (1/ βh 2 )F (q n , t n ) (7.3.17)
To evaluate the Jacobian, derivative operators of Section 5.3 for evaluation of derivatives
of R1 and derivatives of R2 are
564
R 2q&&n = 0
R 2q& n = 0
(7.3.18)
R 2qn = (1 / βh 2 ) F q (q n , t n )
R 2λ n = 0
Using the HHT integration formulas of Eq. (7.2.15) and the chain rule of differentiation,
dR1
is as given in Eq. (7.3.5) and
&& n
dq
dR 2
= F q (q n , t n ) (7.3.19)
&&n
dq
The Jacobian of the residual is thus
é dR1 ù
q
J=ê n ú (7.3.20)
ê dR 2 ú
ê d&& 0 ú
ë qn û
Beginning with initial conditions of Eq. (7.2.1), Eq. (7.3.1) is solved for q &&0 and λ 0 . At
&&n and λ n are estimated as q &&1n = q&&n -1 and
λ1 = λ , or q
n n -1
&&1 = &&
q1 + h&&&
n -1 n n n -1 n q1 and λ& 1 are computed, as in
n n
1
n
1
n
of Eq. (7.3.6) and residual of Eq. (7.3.17) are evaluated as J1 and R1 . The first step in Newton -
J1x = - R1 (7.3.21)
&& = éë I ngc
With Pq 0ùû and Pλ = éë0 I nc ùû , Dq
&&1n = P&&q x, Dλ 1n = Pλ . Solution variables are updated as
&& 2n = q
q &&1n + D q
&&1n
(7.3.22)
λ 2n = λ1n + D λ1n
&&n , and λ n . The process is repeated at t n +1 and continued until
Explicit RKFN45 and implicit HHT integration algorithms may be applied for the Index 1
formulation. For the Index 2 and 3 formulations, the HHT implicit algorithm is required.
Setting a = 0 in the HHT algorithm yields the trapezoidal algorithm.
565
7.4 Code 7.4 for DAE Simulation of Planar Systems
A rudimentary general-purpose MATLAB Code 7.4 has been developed that implements
the DAE formulations of Sections 7.3.1, 7.3.2, and 7.3.3 for planar multibody system simulation.
Revolute, translational, and distance kinematic constraints of Section 3.2 are included, with
derivatives presented in Section 3.2 and Appendix 5.B that are required for implementation of
explicit and implicit numerical integration methods of Section 4.8 for solution of Index 1, 2, and
3 DAE of this chapter. Applied, gravitational, and internal forces defined by translational- and
rotational-spring-damper-actuators (TSDA and RSDA) presented in Section 4.5 are implemented,
together with derivatives of Appendix 5.B. Centroidal body fixed reference frames for equations
of motion derived in Section 4.2 are employed, with derivative expressions presented in
Appendix 5.B. Fixed and variable time step explicit RKFN45 and Nystrom4 and implicit HHT
and trapezoidal integration algorithms are implemented.
Integration, error control, and data input segments of the code are essentially identical to
those of Code 5.7, presented in Section 5.7, so they are only briefly summarized here. Following
an explanation of the computer formulation and MATLAB code in this section, numerical
examples are presented in Section 7.5, including those treated with detailed derivations in
Section 5.4 and with tangent space ODE Code 5.7 in Section 5.8. The focus in numerical
examples is on comparison of results with ODE numerical solution methods.
Components of Code 7.4 of Appendix 7.A that interface with the user are presented in
Section 7.4.1, followed by an outline of the body of the code, with which the user need not
interact, in Section 7.4.2.
The initial segment of code involves integration and error control parameters that
underlie the DAE formulation and associated numerical integration methods. Lines 3 to 12 of
Fig. 7.4.1 define parameters that are used to control error in the DAE formulations and numerical
integration methods. Relatively tight error control parameters are presented as defaults, which
may be modified to seek greater computer efficiency or minimize error. The initial time step is
defined in line 14, followed by the maximum time step allowed in variable time step integrators
in line 16. The final simulation time is defined in line 19. The integration option to be used is
selected from among the four options shown in lines 21 to 24. If alpha is chosen as a nonzero
value in the specified range, the implicit HHT algorithm is implemented. If alpha = 0 is selected,
the implicit trapezoidal algorithm is implemented. For explicit algorithms, no value of alpha is
used.
1 %AA_Planar_Multibody_Sim_DAE
2
3 %Integration and Error Control Parameters
5 Atol=10^-6; %Absolute error tolerance for variable step methods
6 MaxIntiter=8; %Limit on number of implicit integration iterations
7 MaxJcond=100; %Limit on magnitude of Jcond in Trap
8 R1nmax=3000; %Limit on residual in integration
9 MaxECond=20; %Limit on magnitude of ECond
10 PosConstrMax=10^-4; %Limit on position constraint error
11 VelConstrMax=10^-2; %Limit on velocity constraint error
566
12 AccConstrMax=10^20; %Limit on acceleration constraint error
13
14 h0=0.001; %Initial time step
15 h=h0;
16 hmax=0.01; %hvar=1, variable h; hvar=2, constant h
17 hvar=1; %hvar=1, variable h;hvar=2, constant h
18
19 tfinal=10; %Final simulation time
20
21 integ=3; %integ=1-ImplicitIndex1; alpha=0-Trapezoidal; alpha<0-HHt
22 %integ=2-ImplicitIndex2; alpha=0-Trapezoidal; alpha<0-HHt
23 %integ=3-ImplicitIndex3; alpha=0-Trapezoidal; alpha<0-HHt
24 %integ=4-ExplicitRKFN45
25
26 alpha=-1/3; %Enter -1/3<=alpha<0 for HHT
27 %alpha not defined for ExplicitNystrom4 or ExplicitRKFN45
Figure 7.4.1 Integration and Error Control Parameters
Application data are indexed in lines 31 to 41 of Fig. 7.4.2 to specific applications
defined in the AppData function that is defined in the following. The declaration in line 43
defines which application is implemented in the simulation. The function AppData and
parameter definitions of lines 45 through 47 are used throughout the code to pass application
data for each simulation. If required for definition of initial conditions that are consistent with
constraint, the user may enter code following line 49, such as that for the quick return
mechanism of app = 1. Initial accelerations and Lagrange multipliers needed as estimates in
initiating numerical integration are computed in lines 71 to 86. Finally, output data desired are
defined for each application, as in lines 241 to 248 for the quick return mechanism of app = 1.
33 %app=2, Loader
35 %app=4, Multiple Slider-Crank
36 %app=5, Pendulum-Distance constraint
37 %app=6, Pendulum-Revolute constraint
38 %app=7, Pendulum-TSDA
39 %app=8, Double Pendulum
42
43 app=1;
44
45[nb,ngc,nh,nc,NTSDA,NRSDA,PJDT,PMDT,PTSDAT,PRSDAT,q0,qd0]=AppData(a);
46
47 par=[nb;ngc;nh;nc;g;intol;Atol;h0;hvar;NTSDA;NRSDA];
48
50 if app==1
51 phi2d0=6;
53 EEE=[Phiq;zeros(1,5),1,zeros(1,6)];
567
54 RRHS=[zeros(11,1);phi2d0];
55 qd0=EEE\RRHS;
56 end
71 %Calculate Initial Acceleration and Lagrange Multipliers

72 M=MEval(PMDT,par);
74 QA=QAEval(0,q0,qd0,PMDT,PTSDAT,PRSDAT,par);
75 Gam=GamEval(0,q0,qd0,PJDT,par);
76 EE=[M,Phiq';Phiq,zeros(nc,nc)];
77 CondEE=cond(EE);
78 RHS=[QA;-Gam];
79 x=EE\RHS;
80 Pqdd=[eye(ngc),zeros(ngc,nc)];
81 PLam=[zeros(nc,ngc),eye(nc)];
82 qdd=Pqdd*x;
83 Lam=PLam*x;
84
85 Qdd(:,1)=qdd;
86 LLam(:,1)=Lam;

242
243 if app==1
244 phi1(n)=q(3);
245 phi2(n)=q(6);
246 x4(n)=q(10);
247 x4d(n)=qd(10);
248 end
The third component of user entered data is the AppData function, which is identical to
that employed in Codes 3.9 and 5.7 of Sections 3.9.1 and 5.7.1. The reader is referred to that
section for details.
Computational flow in the main program, which requires no input from the user beyond
line 88, is outlined in Fig. 7.4.3. Data that are required for integration methods of Section 7.4.1
are initialized following line 88. Code implementing the four numerical integration methods
supported is outlined following line 108. Corrections to approximate solutions to compensate for
unenforced velocity and position constraints are computed, using the results of Section 8.2.4, in
lines 147 to 180. Total energy for systems simulated is computed following line 188. Output data
of interest for each of the applications discussed in Section 7.4.1 are defined following line 241.
Finally, position, velocity, and acceleration constraint errors are computed following line 350.
88 %Initialize Data For Integration
102 %Integration
104 n=n+1;
568
105 t(n)=t(n-1)+h;
106 tn=t(n);
107 tnm=t(n-1);
108
110 if integ==1 %Implicit Index 1
111 [q,qd,qdd,Lam,R1n,Jiter,JCond]=...
112ImplicitIndex1(n,tn,Q,Qd,Qdd,LLam,h,PMDT,PTSDAT,PRSDAT,PJDT,par,alpha);
116 end
117
124 end
125
132 end
133
134 if integ==4 %Explicit RKFN45
135 [q,qd,qdd,Lam,ECond]=ExplicitNystrom4(n,tn,Q,Qd,...
136 h,PMDT,PTSDAT,PRSDAT,PJDT,par);
138 end
146
147 %Corrections if velocity or position errors exceed tolerances
148 Phiq=PhiqEval(tn,q,PJDT,par);
149 if norm(Phiq*qd)>VelConstrMax
150 mu=(Phiq*Phiq')\Phiq*qd;
151 delqd=-Phiq'*mu;
152 qd=qd+delqd;
153 corvel=corvel+1;
154 corvelrpt(n)=corvel;
155 end
156
157 if norm(PhiEval(tn,q,PJDT,par))>PosConstrMax
158 z=zeros(ngc,1);
159 nu=zeros(nc,1);
160 Pz=[eye(ngc),zeros(ngc,nc)];
161 Pnu=[zeros(nc,ngc),eye(nc)];
162 err=intol+1;
163 i=0;
164 while err > intol
165 Phiq=PhiqEval(tn,q+z,PJDT,par);
166 Resid=[z+Phiq'*nu;PhiEval(tn,q+z,PJDT,par)];
167 I=eye(ngc);
168 JJ=[I+P4Eval(tn,q+z,nu,PJDT,par),Phiq';Phiq,zeros(nc,nc)];
169 w=-JJ\Resid;
170 z=z+Pz*w;
171 nu=nu+Pnu*w;
172 err=norm(Resid);
173 i=i+1;
174 end
175 q=q+z;
569
176 corpos=corpos+1;
177 corposrpt(n)=corpos;
178 corpositer(corpos)=i;
179 end
180 %End Corrections
181
182 %Record Solution
187
188 %Calculate Total Energy
submatrices to sparse matrices, below and to the right of the address of the (1,1) term in the
submatrix added, in the underlying matrix that was initialized to zeroes. ATran evaluates the
orientation transformation matrix. GamEval and GamsqqdEval evaluate terms in Eq. (5.3.18).
Constraint and Data Table Partition Functions listed provide access to elements of the data tables
expressions and the derivatives defined in Section 3.2 and Appendix 5.B that are used in
implementing the formulation and numerical integration of the associated equations of motion.
Care is taken to account for the fact that ground is designated by j = 0 and its generalized
coordinates are constant, yielding no derivative contribution, but to include geometric quantities
that define the constraint of body i with ground. Similarly, the kinetic and derivative evaluation
functions listed evaluate inertial and generalized force terms and derivatives that are defined in
Sections 4.2 and 5.3 and Appendix 5.B and are required in formulation and solution of the
Finally, the numerical integration functions listed carry out the numerical integration
process, where ODEfunct evaluates the Index 1 ODE of Section 7.3.1.1.
Internal details of the MATLAB Functions listed in Fig. 7.4.4 are not presented, since
each of the Functions is documented internally and the user need not modify these functions.
Computing Functions
Add
ATran
GamEval
GamsqqdEval
User Input Function
AppData
parPart
qPart
570
xPart
DistPart
RevPart
TranPart
PTSDATPart
PRSDATPart
PhiEval
PhiqEval
P2Eval
P3Eval
P4Eval
P5Eval
MEval
M2Eval
QAEval
QAsqqd
ExplicitNystrom4
ExplicitRKFN45
ODEfunct
ImplicitTanSpTrap
ImplicitSDIRK54
The rudimentary Code 7.4 of Appendix 7.A is outlined, to enable the reader to create planar
models and carry out simulations using DAE formulations presented in Sections 7.2 and 7.3,
without writing detailed computer code.
571
7.5 Planar System Simulation Using Code 7.4
Index 1, 2, and 3 simulations are carried out using the HHT, trapezoidal, and RKFN45
algorithms for the double pendulum with unilateral spring presented in Section 5.8.1, to study the
influence of stiffness in the model on DAE integration methods. The higher dimensional quick
return mechanism of Section 5.8.2 provides for study of the effectiveness of DAE integration
methods on a slightly more complex example that is not stiff.
The planar double pendulum of Section 5.8.1 is used to evaluate effectiveness of the
Fkfn45, trapezoidal, and HHT integration algorithms, the latter with α = - 1 / 3 , in Index 1, 2, and
3 formulations. The model with unilateral spring in Fig. 5.8.1 serves as a test case for stiff
systems, as in Section 5.8.1. The AppData definition of the model in Code 7.4 of Appendix 7.A
is given in Fig. 7.5.1.
305 if app==7 %Double Pendulum-Unilateral Spring

306
311 nv=ngc-nc;
312 nu=nc;
315
316 ux=[1;0];
317 uy=[0;1];
318 zer=zeros(2,1);
319
323 %si&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=vectors along trans axis
326
328 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
329 PMDT=[[1;0.3],[1;0.3]];
330
335 %Unilateral spring defined by setting spring constant in QA, QAsq,
336 %and QAsdd equal to K=K*(1-sign(q(1)))/2
337 K3=10^5;
338 PTSDAT(:,1)=[1;0;zer;-ux-uy;K3;0;1;0];
339
572
344 PRSDAT(:,1)=[1,0,0,0,pi/2,0];
345 PRSDAT(:,2)=[1,2,10^5,10^4,0,0];
346
348
349 q0=[1;0;0;3;0;0];
350 qd0=[0;0;0;0;0;0];
351
352 end
Figure 7.5.1 AppData, Double Pendulum with Unilateral Spring
In successful simulations with a variety of spring and damper values, results are
consistent with those presented in Section 5.8.1. Success and failure of 2 sec simulations carried
out with Code 7.1 and data of Section 5.8.1, with intol = e-6, Atol = e-4, and hmax=e-3, are
denoted “s” and “x”, respectively, in Tables 7.5.1 through 7.5.3 for variable step size trapezoidal
and HHT algorithms and Index 1, 2, and 3 formulations. Stiffness and damping parameters used
in simulations are listed in the last three rows of Tables 7.5.1 through 7.5.3. Results are reported
for the explicit RKFN45 algorithm in the Index 1 formulation, where it succeeds only for
moderately stiff models.
For the Index 1 formulation, both the trapezoidal and HHT algorithms provide good
approximate solutions for quite stiff systems, ultimately failing with the error control algorithm
driving step size to zero. The trapezoidal algorithm requires smaller step sizes for very stiff
models than the HHT algorithm, with its algorithmically induced damping.
Table 7.5.1 Index 1 Simulation Success and Failure with Stiff Systems
RKFN45 s s s x x x
Trapezoidal s s s s s x
HHT s s s s x x
K2 e3 e4 e5 e6 e7 e8
C2 e2 e3 e4 e5 e6 e7
K3 e3 e4 e5 e6 e7 e8
For the Index 2 formulation, the trapezoidal and HHT algorithms again perform similarly,
with trapezoidal able to treat somewhat stiffer systems. For the stiffest model in which both
algorithms succeed, the trapezoidal algorithm required an average step size of 2.6e-5, compared
to 3.7e-4 for the HHT algorithm. On the other hand, the HHT algorithm predicts slightly lower
amplitude oscillation, probably due to the higher level of algorithmic damping.
573
Trapezoidal s s s s s s x
HHT s s s s x x x
K2 e3 e4 e5 e6 e7 e8 e9
C2 e2 e3 e4 e5 e6 e7 e8
K3 e3 e4 e5 e6 e7 e8 e9
For the Index 3 formulation, the Petzold Imperative appears to have taken control. As
shown in Table 7.5.3, the trapezoidal algorithm fails for even the least stiff case, whereas the
HHT algorithm yields acceptable results for a significant range of stiffness. This behavior is the
reason the HHT algorithm for multibody dynamics was created (Negrut, Rampalli, Ottersson,
and Sajdak, 2007). The introduction of algorithmic damping appears to result in a convergent
algorithm in the Index 3 formulation, whereas the trapezoidal algorithm fails.
Trapezoidal x x x x
HHT s s s x
K2 e3 e4 e5 e6
C2 e2 e3 e4 e5
K3 e3 e4 e5 e6
The behavior portrayed in Table 7.5.3 is often used in the literature to suggest that the
equations of motion of multibody dynamics are inherently stiff. This is clearly not the case. The
ODE formulation of Chapter 5 shows no sign of inherent stiffness. Similarly, the Index 1 and 2
DAE formulations show no inherent stiffness and are able to effectively treat even the current
application that, with large spring rates and damping coefficients, becomes stiff in the limit.
As a test of the ability of Index 1, 2, and 3 formulations to control constraint error, the
medium stiffness case with K2 = K3 = e5 and C2 = e4 is simulated with trapezoidal and HHT
algorithms for 10 sec, with intol = e-6, Atol = e-4, and hmax=e-3. Maximum errors in norms of
position, velocity, and acceleration constraints are presented in Tables 7.5.4 and 7.5.5. For all
unenforced constraints, maximum norms of constraint error are several orders of magnitude
larger than errors reported for the same system using the tangent space ODE formulation. For
both algorithms, the precision in solution of the Index 2 formulation appears to be comparable to
that of the Index 3 formulation. In light of theoretical and practical difficulties with the Index 3
formulation, these results suggest that the Index 2 formulation may be the preferable alternative.
574
Table 7.5.4 HHT Simulation Errors, K2 = K3 = e5 C2 = e4
Position Velocity Acceleration
Index 1 4e-3 8e-4 6e-14
Index 2 3e-8 2.5e-15 1.2e-1
Index 3 7e-16 5.5e-6 1e-1
Table 7.5.5 Trapezoidal Simulation Errors, K2 = K3 = e5 C2 = e4

Position Velocity Acceleration
Index 1 1.4e-3 3e-4 2e-13
Index 2 3.5e-6 2e-15 8e-2
The quick return mechanism of Section 5.8.2 serves as a higher dimensional application
to test numerical methods in Index 1, 2, and 3 DAE formulations. Model and initial condition
data are as in Section 5.8.2, given in the AppData function of Code 7.4 in Appendix 7.A, shown
in Fig. 7.5.2. Error control parameters are selected as intol = e-8, Atol = e-6, and hmax=e-3.
Simulation results with the Index 1 and 2 formulation and trapezoidal and HHT algorithms, for
an initial angular velocity 6 rad/sec of the crank, are as in Fig. 5.8.8 of Section 5.8.2. Identical
results are obtained with the explicit RKFN45 algorithm applied to the Index 1 formulation,
confirming the conclusion of Section 5.8.2 that this system is not stiff. Simulations with the
Index 3 formulation and the trapezoidal algorithm with constant step size h = 0.001 and variable
step size failed. The HHT algorithm with constant step size h = 0.002 succeeded, but with
warnings that integration error may be large. The fact that the system is not stiff suggests that the
Petzold Imperative has again intervened, emphasizing that the Index 3 formulation is indeed
problematic.

5
14
15 ux=[1;0];
16 uy=[0;1];
17 zer=zeros(2,1);
18
575
22 %si&jpr=vectors to Pi&j, d=dist., vi&jpr=vectors along trans
axis
29
31 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
32 PMDT=[[100;100],[1000;1000],[1;1],[50;50]];
33
36 %i&j=bodies connected, si&jpr=vectors to Pi&j, K=spring
constant,
39
45
47 q10=[1.2;1.6;0.9273];
48 q20=[0;2;0];
49 q30=[1.5;2;0.9273];
50 q40=[0;4;0];
51
52 q0=[q10;q20;q30;q40];
54
55 end
Figure 7.5.2 AppData, Quick Return Mechanism
To evaluate the influence of varying initial crank angular velocity on integration
algorithm performance, simulations were run with the foregoing data and initial crank angular
velocities 6, 12, and 18 rad/sec. Results for initial angular velocity 6 rad/sec were identical to
those presented in Fig. 5.8.7. Results for initial angular velocity 12 and 18 rad/sec are with twice
and three times the frequency, the same amplitude, twice and three times the peak velocities, and
four and nine times the peak accelerations. Data provided in Tables 7.5.6 and 7.5.7 indicate that
both algorithms perform well, even for the more extreme dynamic response associated with the
higher initial angular velocities. In the Index 1 formulation, the trapezoidal algorithm satisfies
unenforced constraints two orders of magnitude more precisely than the HHT formulation.
576
Table 7.5.6 Constraint Error and Average Step Size, Index 1 Formulation
f&2 (0) Pos err Vel err Acc err h-ave
Trapezoidal
6 4.5 e-4 5 e-4 5 e-13 5 e-4
12 3.3 e-3 2.5 e-3 1.1 e-12 2.6 e-4
18 9 e-3 7 e-3 2.5e-12 1.7 e-4
HHT
6 9 e-2 6 e-2 2 e-13 2.1 e-4
12 3 e-1 2.4 e-1 9 e-12 9.3 e-5
18 6.5 e-1 5 e-1 3.5e-12 5.7 e-5
Table 7.5.7 Constraint Error and Average Step Size, Index 2 Formulation
f&2 (0) Pos err Vel err Acc err h-ave
Trapezoidal
6 4.5 e-5 2 e-14 2 e-2 5.1 e-4
12 1.2 e-4 4 e-14 6 e-2 2.4 e-4
18 1.8 e-4 9e-14 1.5e-1 1.5 e-4
HHT
6 3.5 e-5 2 e-14 3.5 e-1 2.1 e-4
12 4 e-5 4 e-14 1.5 9.6 e-5
18 4.5e-5 8 e-14 3 6.1 e-5
To gain insight into difficulties with the Index 3 formulation, the HHT algorithm is
applied with initial angular velocity of the crank equal to 6 rad/sec, error control parameters
defined above, h = 0.001, and tfinal = 0.5 sec. The plot of &x&4 = x4dd for the slider in Fig. 7.5.3
shows that it is initially highly oscillatory, but damps to a response similar to that predicted in the
Index 1 and 2 formulations in Fig. 7.5.4, obtained with HHT, trapezoidal, and RKFN45
algorithms. With a step size h = 0.002, the initial transient is much less severe, suggesting that
the Index 3 formulation is sensitive to small step size.
577
Figure 7.5.3 x4dd vs Time, HHT Index 3, a = - 1 / 3
Figure 7.5.4 x4dd vs Time, HHT Index 2, a = - 1 / 3
Simulation results for increasingly stiff models of the double pendulum show that both the
HHT and trapezoidal algorithms yield reliable results with Index 1 and 2 formulations. With
the Index 3 formulation, the trapezoidal algorithm with variable step size fails, whereas the
HHT algorithm marginally succeeds. For the higher dimensional quick return mechanism, all is
well in the Index 1 and 2 formulations, but all simulations fail or display large error in the
Index 3 formulation.
578
7.6 Code 7.6 for DAE Simulation of Spatial Systems
A rudimentary general-purpose Code 7.6 has been created that implements the DAE
formulation of Sections 7.3.1, 7.3.2, and 7.3.3 for spatial multibody system simulation.
Kinematic constraints of Section 3.3 are included, with derivatives presented in Section 3.3 and
Appendix 5.B that are required to implement explicit and implicit numerical integration methods
of Section 4.8 for solution of Index 1, 2, and 3 DAE of this chapter. Applied, gravitational, and
internal forces defined by translational-spring-damper-actuators (TSDA) presented in Section
4.5 are implemented. Centroidal body fixed reference frames for equations of motion derived in
Section 4.3 are employed. Fixed time step explicit RKFN45 and implicit HHT and trapezoidal
methods are implemented.
Integration and error control and data input segments of the code are essentially identical
to those of Code 7.4, so they are only briefly summarized here. Following an explanation of the
computer formulation and MATLAB code in this section, numerical examples are presented in
Section 7.7, including those treated with detailed derivations in Section 5.4 and with tangent
space ODE Code 5.9 in Section 5.10. The focus in numerical examples is to compare results with
ODE numerical solution methods.
Components of Code 7.6 of Appendix 7.A that interface with the user are presented in
Section 7.6.1, followed by an outline of the body of the code, with which the user need not
interact, in Section 7.6.2.
The initial segment of code involves integration and error control parameters that
underlie the DAE formulation and associated numerical integration methods in lines 1 to 26.
Since this segment of code is essentially identical to that in Section 7.4.1, the reader is referred to
that section.
Application data are indexed in lines 30 to 36 of Fig. 7.6.1 to specific applications
defined in the App Data function defined in the following. The declaration in line 38 defines
which application is implemented in the simulation. The AppData and parameter definitions of
lines 31 through 36 are used throughout the code to pass application data for each simulation. If
required for definition of initial conditions that are consistent with constraint, the user may enter
code following line 44. Initial accelerations and Lagrange multipliers that are needed as
estimates in initiating numerical integration are computed in lines 60 to 76. Finally, output data
desired are defined for each application, as in lines 214 to 220 for the pendulum of app = 1.
31 %app=1, Pendulum, Spherical to Ground
32 %app=2, Top, Spherical to Ground
33 %app=3, One Body Pendulum, Dist. to Ground
34 %app=4 Two Body Pendulum
35 %app=5, One Body Cylindrical with Spring
36 %app=6, Spatial Slider-Crank
37
38 app=2;
39
40 [nb,ngc,nh,nc,NTSDA,SJDT,SMDT,STSDAT,q0,qd0]=AppData(app);
579
41
42 par=[nb;ngc;nh;nc;g;intol;Atol;h0;hvar;NTSDA];
43
60 %Calculate Initial Acceleration and Lagrange Multipliers

61 M=MEval(q0,SMDT,par);
62 Phiq0=PhiqEval(0,q0,SJDT,par);
63 QA=QAEval(0,q0,qd0,SMDT,STSDAT,par);
64 S=SEval(q0,qd0,SMDT,par);
65 Gam=GamEval(0,q0,qd0,SJDT,par);
66 EE=[M,Phiq0';Phiq0,zeros(nc,nc)];
67 EEcond=cond(EE);
68 RHS=[QA+S;-Gam];
69 x=EE\RHS;
70 Pqdd=[eye(ngc),zeros(ngc,nc)];
71 PLam=[zeros(nc,ngc),eye(nc)];
72 qdd=Pqdd*x;
73 Lam=PLam*x;
74
75 Qdd(:,1)=qdd;
76 LLam(:,1)=Lam;

215
216 if app==1
217 x1(n)=q(1);
218 y1(n)=q(2);
219 z1(n)=q(3);
220 end
The third component of user entered code is the AppData Function, which is identical to
that employed in Code 5.7 and defined in Fig. 5.7.2 of Section 5.7.1. The reader is referred to
that section for details.
Computational flow in the main program, which requires no input from the user, is
identical to that of Code 7.1 and is outlined in Fig. 7.4.2. The reader is referred to Section 7.4.2
for details.
submatrices to sparse matrices, below and to the right of the address of the (1,1) term in the
submatrix added, in the underlying matrix that was initialized to zeroes. ATran evaluates the
orientation transformation matrix. GamEval and GamsqqdEval evaluate terms in Eq. (5.3.18).
Constraint and Data Table Partition Functions listed provide access to elements of the data tables
580
expressions and derivatives defined in Section 3.3 and Appendix 5.B that are needed in
implementing the formulation and numerical integration of the associated equations of motion. A
substantial number of functions are included for building block constraints under the heading
bbxxx. Care is taken to account for the fact that ground is designated by j = 0 and its generalized
coordinates are constant, yielding no derivative contribution, but including geometric quantities
that define a constraint of body i with ground. Similarly, the kinetic and derivative evaluation
functions listed evaluate inertial and generalized force terms and derivatives defined in Sections
4.3 and 5.3 and Appendix 5.B that are required in the DAE formulation and solution of the
Finally, numerical integration functions listed carry out the numerical integration process,
where ODEfunct evaluates the Index 1 ODE of Section 7.3.1.1.
Internal details of the MATLAB Functions listed in Fig. 7.6.2 are not presented, since
each of the Functions is documented internally and the user need not modify these functions in
applications.
Computing Functions
Add
atil
EEval
GEval
KEval
BTran
ATran
GamEval
GamsqqdEval
User Input Function
AppData
parPart
qPart
xPart
pNormPart
CylPart
DistPart
RevPart
TranPart
StrutPart
RevSphPart
UnivPart
STSDATPart
bbxxx
PhiEval
PhiqEval
P2Eval
P3Eval
581
P4Eval
P5Eval
MEval
M2Eval
QAEval
QAsqqd
ExplicitNystrom4
ExplicitRKFN45
ODEfunct
ImplicitTanSpTrap
ImplicitSDIRK54
Jacob
Resid
The rudimentary Code 7.6 of Appendix 7.A is outlined, to enable the reader to create spatial
models and carry out simulations, without writing detailed computer code.
582
7.7 Spatial System Simulation Using DAE Code 7.6
Three spatial examples that are simulated using tangent space ODE Code 5.9 of
Appendix 5.A are treated here using DAE Code 7.6. Kinematic and kinetic characteristics of the
models are defined in AppData functions, just as in Section 5.9. Code 7.6 is then run and results
compared with those of the ODE formulation.
7.7.1 Spin Stabilized Top
The spin stabilized top studied in Sections 5.1, 5.4.2.1, 5.6.1.1, and 5.10.1 is simulated
using the DAE approach in Code 7.6. Plots of the x-y trajectory and of total energy vs time for
initial angular velocity of 13.5 rad/sec are presented in Fig. 7.7.1. The plot of x-y trajectory at the
bottom left that is obtained using the Index 1 formulation with the trapezoidal integrator is
virtually identical to results presented in Figs. 5.1.3 and 5.10.2. Consistent with the conservative
character of this system, the total energy plot at the bottom right of Fig. 7.7.1 shows that
conservation of energy is accurately accounted for by the trapezoidal integrator. Identical results
are obtained using the trapezoidal integrator with the Index 2 formulation and the RKFN45
integrator with the Index 1 formulation. The system is therefore clearly not stiff. Nevertheless,
the trapezoidal integrator fails in the Index 3 formulation. This is a clear indicator that instability
of the trapezoidal approach in the Index 3 DAE formulation is a result of the Petzold Imperative;
i.e., DAE are not ODE.
In contrast to the foregoing, results presented in the first three rows of Fig. 7.7.1 using the
HHT integrator in all three Index formulations are completely inaccurate. This is due to the
dominant effect of numerical damping introduced in the HHT formulation to enhance numerical
stability, resulting in the loss in total energy shown in the plots on the right as time proceeds.
Constraint error data shown in the text box of the right plots also explain the poor performance
of the HHT integrator over long time simulations. These difficulties arise in this application, due
in part to the long simulation time that is required to establish convergence to the spin stabilized
vertical trajectory. In an attempt to compensate for these difficulties, the velocity correction
scheme of Section 7.2.4 is incorporated in the Index 1 HHT formulation. While this leads to a
reduction in loss of total energy, the approximate solution is totally corrupted. This is likely a
result of the Larry Bird Paradox of Section 7.2.
583
Figure 7.7.1 Spin Stabilized Top with omegaz0 = 13.5 rad/sec
584
The spatial double pendulum studied in Sections 5.4.4, 5.6.2, and 5.10.2 is simulated
using the DAE approach in Code 7.6. Results using intol = e-6, Atol = e-5, and hmax = e-3 over
a 20 sec simulation with RKFN45, trapezoidal, and HHT integrators in the Index 1 formulation,
the trapezoidal and HHT integrators in the Index 2 formulation, and the HHT integrator in the
Index 3 formulation are essentially identical to those reported using tangent space ODE and
Index 0 DAE formulations in Sections 5.4.4, 5.6.2, and 5.10.2. The fact that the explicit RKFN45
integrator succeeds indicates that the system is not stiff. Nevertheless, the trapezoidal integrator
fails in the Index 3 formulation. This is another invocation of the Petzold Imperative.
To assess the ability of integrators in the various Index formulations to successfully
enforce constraints, maximum norms of position (configuration), velocity, and acceleration
constraint errors are presented in Table 7.7.1. Interestingly, the high order explicit RKFN45
integrator led to several orders of magnitude less error in unenforced constraints in the Index 1
formulation, contrasted with the poor performance of the HHT integrator. In the Index 2
formulation, the trapezoidal and HHT formulations exhibited similar performance. In the Index 3
formulation, the HHT integrator exhibited good results and the trapezoidal integrator failed.
Table 7.7.1 Spatial Double Pendulum Simulation Constraint Error
RKFN Ind 1 HHT Ind 1 Trap Ind 1 HHT Ind 2 Trap Ind 2 HHT Ind 3
Pos Err 3e-8 1.2e-1 9e-6 4e-6 3.8e-6 8e-16
Vel Err 2.7e-9 1.3e-2 6e-5 8e-16 1e-15 4e-5
Acc Err 4e-13 2.5e-14 2.5e-14 1.3e-1 9e-3 1.4e-1
7.7.3 Slider-Crank
The spatial slider-crank studied in Section 5.10.4 is simulated using the DAE approach
in Code 7.6. Results using intol = e-6, Atol = e-5, and hmax = e-3 over a 0.25 sec simulation
with RKFN45, trapezoidal, and HHT integrators in the Index 1 formulation, the trapezoidal and
HHT integrators in the Index 2 formulation, and the HHT integrator in the Index 3 formulation
are essentially identical to those reported in Section 5.10.4, using tangent space ODE and Index 0
DAE formulations. The fact that the explicit RKFN45 integrator succeeded indicates that the
system is not stiff. Nevertheless, the trapezoidal integrator failed in the Index 3 formulation;
another invocation of the Petzold Imperative.
Maximum norms of position, velocity, and acceleration constraint errors presented in
Tables 7.7.2 and 3 show the same trends as in Table 7.7.1. Perhaps a bit surprising, the
integrators performed well even for the near singular system with d3 = 0.23 m, according to data
in Table 7.7.4.
585
Table 7.7.2 Spatial Slider-Crank Simulation Constraint Error, d3=0.4 m
Pos Err 6e-8 1e-3 8e-4 4.6e-7 7.5e-6 1.1e-16
Vel Err 3e-7 9e-3 6e-3 1e-14 1.2e-14 2e-5
Acc Err 1e-12 9e-13 1.4e-12 6e-1 3e-1 7e-1

Pos Err 1.3e-7 6.5e-4 1.5e-3 4.3e-7 1.7e-5 1.1e-16
Vel Err 9e-7 9e-3 1.2e-2 1e-14 1.2e-14 4e-5
Acc Err 9e-13 1.4e-12 1.4e-12 1 3e-1 1

Pos Err 2e-6 7.5e-3 1.6e-3 2.5e-6 1.3e-5 2e-16
Vel Err 1.8e-5 6e-2 1.4e-2 1.3e-14 1.6e-14 1.2e-4
Acc Err 1.6e-12 1.8e-12 1.6e-12 3 3.7 3.5
Long time simulation results for the spinning top show that numerical damping embedded in
the HHT integrator to enhance stability leads to erroneous results, in contrast to good results
obtained with the explicit RKFN45 and implicit trapezoidal integrators.
Simulation results for the spatial double pendulum and slider-crank models show that
RKFN45, HHT, and trapezoidal integrators yield reliable results with the Index 1 formulation,
as do HHT and trapezoidal integrators in the Index 2 formulation. With the Index 3
formulation, the trapezoidal and RKFN45 algorithms fail, whereas the HHT integrator
succeeds, as suggested by the Petzold Imperative.
586
Appendix 7.A DAE Solution Code
Code 7.4 Planar Multibody Simulation-DAE

Code 7.6 Spatial Multibody Simulation-DAE
587
588
CHAPTER 8
Simulation of Friction in Multibody Dynamics
8.0 Introduction
Modeling friction in multibody system simulation is a daunting task that has been
pursued for decades, with no comprehensive solution to date. Progress in this regard is impeded
by the complexity of quantifying constraint reaction forces and representing friction forces as
functions of constraint forces, relative velocities between contact points and surfaces in joints,
and constitutive relations involving material interaction due to contact and viscous effects of
lubricants. A second factor impeding progress in simulation of multibody systems with friction is
apparent stiffness of Index 3 DAE of motion, due to friction effects, that has been reported in the
literature. Approaches used in this chapter to address some of the challenges noted include (1)
implementing continuous approximations of friction effects, including stiction; (2) evaluating
continuous friction approximations relative to models of discontinuous Coulomb friction; and (3)
utilizing the index 0 DAE formulation to avoid apparent stiffness.
A continuous model of friction is used in Section 8.1 with the tangent space Index 0 DAE
formulation of Section 5.5 for simulation of a three-mass model problem. Analytical criteria for
stiction are derived for the model problem in Section 8.2, using the discontinuous Coulomb
model to define the onset of and departure from stiction events with redundant equations of
constraint. The tangent space formulation with implicit trapezoidal integration is applied to this
analytical model to compute dynamic response, determine ranges of constraint forces that may
occur during periods of stiction, and demonstrate that dynamic response is a discontinuous
function of model parameters when stiction occurs. Cartesian coordinate models of higher
dimension are presented in Section 8.3, for three and four mass model problems with continuous
friction models that encounter redundancy in constraints during periods of stiction.
Formulations for modeling and simulation of friction effects in planar and spatial
multibody systems are presented in Sections 8.4 and 8.5, using kinematic constraints presented in
Sections 3.2 and 3.3. Constraint reaction forces on bodies that are connected by joints that
support friction are derived as functions of Lagrange multipliers. Friction forces that act on
bodies are calculated as functions of joint geometry, constraint reaction forces that are functions
of Lagrange multipliers, and relative velocities at constraint contact points that are determined by
system kinematics. Friction forces are introduced into the Index 0 DAE formulation of system
equations of motion in Section 8.6.
Rudimentary general-purpose Codes 8.7. and 8.9 for simulation of planar and spatial
systems with friction are presented in Sections 8.7 and 8.9, respectively. Numerical examples of
planar and spatial system simulation with friction are presented in Sections 8.8 and 8.10,
reinforcing conclusions drawn in simulation of more modest model problems in prior sections.
MATLAB computer codes for all applications treated are presented in Appendix 8.A and key
formulas for the chapter are tabulated in Appendix 8.B.
589
8.1 Modeling Friction in Multibody Dynamics
Modeling friction in multibody system simulation is a daunting task. For deformable
bodies, the challenge is of monumental proportion, as outlined in a literature review by Berger
(2002). Even for rigid bodies, many models have been proposed (Marques, Flores, Pimenta
Claro, and Lankarani, 2016; Pennestri, Rossi, Salvini, and Velentini, 2016; Brown and McPhee,
2016), but their effective implementation in a multibody dynamics setting remains an open
challenge. Progress in this regard is impeded by the complexity of quantifying constraint
reaction force and representing friction force as a function of constraint geometry and reaction
forces, relative velocities between contact points and surfaces in joints, and constitutive relations
involving material interaction due to contact and viscous effects of lubricants.
A second factor impeding progress in simulation of multibody systems with friction is
apparent stiffness of Index 3 differential-algebraic equations (DAE) of motion due to friction
effects, as reported in the literature cited above. Resolving these issues, in the context of spatial
multibody dynamics, is a near-impossible task. The objective of this Section is to formulate and
analyze multibody static and dynamic model problems that embody challenges in modeling and
simulation of friction effects, without the extreme geometric and analytical complexity of spatial
multibody systems. Specific challenges addressed include (1) implementing continuous
approximations of friction effects, including stiction; (2) evaluating continuous approximations
relative to models that represent discontinuous effects; and (3) utilizing the tangent space Index 0
DAE formulation of Chapter 5 to account for nonlinearities due to friction.
8.1.1 Static Coulomb Friction in a Constrained System
As an elementary precursor to analysis of friction in a dynamic setting, consider two
blocks on a horizontal plane that are connected by a massless bar and acted on by an applied
force F ³ 0 , as shown in Fig. 8.1.1. With Coulomb friction acting between the blocks and plane
and with the blocks at rest, the objective is to determine the smallest value of F that leads to
breaking the static friction, or stiction, bonds between the blocks and plane and leads to sliding
motion.
Figure 8.1.1 Constrained Blocks on Horizontal Plane

For blocks at rest, with free body diagrams shown in Fig. 8.1.1, T is tension in the bar
that connects the blocks, and normal forces between the blocks and plane are N1 = N 2 = mg .
Necessary and sufficient conditions for the blocks to remain at rest are equilibrium conditions
T - sf1 = 0 and F - T - sf 2 = 0 and Coulomb friction inequalities sf i / N i £ μ s , i = 1,2, where μ s
is the coefficient of static friction. Stiction forces are thus sf1 = T and sf 2 = F - T and Coulomb
friction limits on stiction forces are
590
T = sf1 £ ms N1 = ms mg
(8.1.1)
F - T = sf 2 £ ms N 2 = ms mg
For a given applied force F > 0 , stiction occurs if and only if the set of T that satisfy the
inequalities of Eq. (8.1.1) is nonempty. Otherwise the set of feasible values of T is empty and
stiction cannot be supported. This may be viewed as an implication of Newton’s first law; i.e.,
bodies at rest tend to stay at rest, only making the transition to nonzero motion if applied forces
are sufficiently large.
Inequalities equivalent to those of Eq. (8.1.1) are
-ms mg £ T £ ms mg
(8.1.2)
F - ms mg £ T £ F + ms mg
The set of all T that satisfy the inequalities of Eq. (8.1.2), equivalently Eq. (8.1.1), is defined by
the inequalities max(-ms mg, F - ms mg) £ T £ min(ms mg, F + ms mg) . Since F ³ 0 , a necessary and
sufficient condition for equilibrium is
F - ms mg £ T £ ms mg (8.1.3)
If F > 2ms mg , the set of T that satisfy the Coulomb friction inequalities of Eqs. (8.1.3) is empty,
stiction is not possible, and the blocks will slide. If F £ 2ms mg , the set of T that satisfy the
coulomb friction inequalities of Eqs. (8.1.3) is not empty, the blocks are in equilibrium,
acceleration is zero, and the blocks remain at rest; i.e., stiction occurs.
A point of interest with this example is that the largest value of F such that the set defined
by Eqs. (8.1.3) is not empty causes both inequalities of Eqs. (8.1.1) to be equalities; i.e., with
F = 2ms mg and T = ms mg from Eq. (8.1.3), the inequalities of Eqs. (8.1.1) reduce to
sf1 = T = ms mg = ms N1
(8.1.4)
sf 2 = F - T = 2ms mg - ms mg = ms mg = ms N 2
8.1.2 Three Mass Model with Friction and Stiction
8.1.2.1 Mechanism Model
The mechanism shown in Fig. 8.1.2 is comprised of three lumped masses that slide along
orthogonal x and y axes in a vertical plane, acted on by a distance constraint between masses
m1 and m2 , linear springs, gravitational force in the negative y direction, and Coulomb friction.
Generalized coordinates are q = [ q1 q 2 q 3 ] shown in Fig 8.1.2, constrained by a massless rod
T
of length l between masses one and two; i.e., the holonomic constraint
Φ(q) = ( q12 + q 22 - l 2 ) / 2 = 0 (8.1.5)
The Jacobian of this constraint is Φq = [q1 q 2 0] .
591
Figure 8.1.2 Three Mass Planar Mechanism
From Eq. (4.10.8), without Euler parameters, the generalized force that acts on the system
due to the constraint between masses one and two is
Q C = -lF Tq = - λ [ q1 0]
T
q2 (8.1.6)
Since δW = δ TqQC = -lq1δq1 - lq 2δq 2 , force -lq1 acts on mass m1 along the y axis and force
-lq 2 acts on mass m 2 along the x axis. These constraint forces, gravitational forces, spring
forces acting on the masses, and reaction forces that act on the massless bar are shown in Fig.
8.1.3. Since the bar is massless, it is required that reaction forces on the bar due to contact with
masses one and two are F1 = - λq 2 and F2 = -λq1 . When the masses are in motion, with a
dynamic coefficient of friction μd acting between the axes and masses, Coulomb friction forces
on masses one, two, and three are -μ d N1sign(q& 1 ) , -μ d N 2sign(q& 2 ) , and -μ d m3gsign(q& 3 ) ,
respectively. Free body diagrams of the masses and the massless bar in Fig 8.1.3 yield
N1 = F1 = - λq 2
(8.1.7)
N 2 = F2 + m 2 g = m 2 g - λq1
and the equations of motion
é q1 ù é - m1g - k1q1 ù é -μ d λq 2 sign(q& 1 ) ù
&& + q 2 λ = êê k 2 ( q3 - q 2 - 1) úú + êê -μ d λq1 - m 2 g sign(q& 2 ) úú º Q A
Mq ê ú (8.1.8)
ê ú
êë 0 úû êë - k 2 ( q 3 - q 2 - 1)úû êë -μ d m3gsign(q& 3 ) úû
where M = diag(m1 , m2 , m3 ) .
Figure 8.1.3 Free Body Diagrams
592
8.1.2.2 Continuous Friction-Slip Velocity Function
In order to avoid difficulties with the physically unrealistic discontinuous friction effects
when sliding velocity is near zero (Brown and McPhee, 2016; Marques, Flores, Claro, and
Lankarani, 2016; Pennestri, Rossi, Salvini, and Velentini, 2016), a continuous friction
approximation with local variation in magnitude near q& = 0 is used. The model adopted was
developed by Brown and McPhee (2016) to account for phenomena that lead to static and
dynamic friction. As a function of model and system parameters, the continuous friction force in
this model is
Ff = -Fn (q, λ )S(v,μ) (8.1.9)
where Fn (q, λ ) is the absolute value of normal force due to contact, as a function of generalized
coordinates and Lagrange multipliers that depend on system kinematics and the DAE of motion,
and
S(v,μ) = m d tanh(4v / v t ) + (m s - m d )(v / v t ) / ( (v / 2v t ) 2 + 3 / 4 )

2
(8.1.10)
where v is slip velocity, v t is a transition velocity (2 v t is the difference between velocities at

maximum and minimum transition friction force), and μ = [m s m d ] , where md is the coefficient
T
of dynamic friction and m s is the coefficient of static friction. The graph of this function, for
Fn (q, λ ) = 1 , vt = 0.01 , md = 0.3 , and μ s = 0.5 is presented in Fig. 8.1.4, showing that friction
force is negative for positive slip velocity and positive for negative slip velocity.
Figure 8.1.4 Continuous Friction Force Function

This model is more realistic for mechanical systems with some lubrication than the
classical discontinuous Coulomb friction model. With some form of lubricant in zones of contact
for realistic mechanical systems, the Coulomb discontinuous model is a fantasy. While the
continuous model suggested is an idealization of physical reality, it is superior to the unrealistic
discontinuous Coulomb model.
The derivative of the friction force function of Eq. (8.1.9) with respect to v, using
subscript notation for derivative, is
593
Ff v = -Fn (q, λ )S¢(v,μ) (8.1.11)
where
S¢(v,μ ) º S(v,μˆ )v = (4md / v t ) (1 - tanh 2 (4v/v t ) ) + (ms - m d )(1 / v t ) / ( v/2v t )2 + 3 / 4 )

2
(8.1.12)
- (ms - md )(v 2 / v 3t ) / ( v/2v t )2 + 3 / 4 )
3
As in Eq. (8.1.8), the normal force Fn (q, λ ) is often the absolute value of an expression in
q and l ; e.g., in Eq. (8.1.7), Fn (q, λ ) = N1 (q, l) = lq 2 . Since x = xsign(x) , the derivative
d x / dx is discontinuous. A continuously differentiable approximation of the function sign(x),
consistent with the friction force approximation of Eq. (8.1.10), is
ìsign(x), if x > v t
ï
x » csign(x) º í æ px ö (8.1.13)
ï sin ç 2v ÷ , if x £ v t
î è tø
with derivative
ì0, if x > v t
ï
d x /dx » dcsign(x) º í p æ px ö (8.1.14)
ï 2v cos ç 2v ÷ , if x £ v t
î t è tø
8.1.2.3 Tangent Space Index 0 DAE Model with Friction
With the friction approximation of Section 8.1.2.2, equations of motion with friction that
generalize those of Section 8.1.2.1 are
&& + F q (q )T l - Q A (q, q& , λ ) = 0
M(q )q (8.1.15)
with kinematic configuration, velocity, and acceleration constraints

F(q) = 0
Fq q& = 0 (8.1.16)
Fq && ( )
q + F qq&ˆ q& º Fqq
q
&& + g = 0
The tangent space parameterization of Section 5.2 remains valid and is not influenced by
the dependence of generalized force on Lagrange multipliers. Basic relations of Section 5.2 are
summarized here, for use in treating systems with friction. Since the constraint Jacobian Fq
must have full rank, singular value decomposition may be used at the initial configuration
(t 0 , q0 ) to define an orthogonal basis V for its null space, F q (q 0 )V = 0 and V T V = I . The
columns of V and those of U º F q (q0 )T form a basis for configuration space, so any vector of
generalized coordinates can be uniquely written in the tangent space form of Eq. (5.2.9),
q = q0 + Vv - Uu (8.1.17)
594
In order that q satisfies the first of Eqs. (8.1.16), and noting that the matrix F q (q 0 )F qT (q 0 ) is
nonsingular, B (q ) = ( F q (q, t) U )
-1
is defined in a neighborhood of q0 and u is uniquely
determined as the solution of
F ( q0 + Vv - Uu ) = 0 (8.1.18)
which exists and is unique. Since F u ( q ) = -F q ( q ) U º -B(q )-1 is nonsingular, a unique

solution exists, as a function of v,
u = h( v ) (8.1.19)
Using this result in Eq. (8.1.17),
q = q0 + Vv - Uh( v) (8.1.20)
The matrix B and solution u = h( v ) are evaluated using Newton-Raphson iteration in Eqs.
(5.2.20) and (5.2.17), as follows:
Bi +1 = 2Bi - BiF q UB, i=1,2,L, until Fq UBi +1 - I £ Btol (8.1.21)
u i+1 = ui + B(q, t)F ( q 0 + Vv - Uu i , t), i = 1,2,... until F ( q0 + Vv - Uu i +1 , t) £ utol (8.1.22)
Linear algebra relations derived in Section 5.2 yield velocities and accelerations of Eqs. (5.2.34)
and (5.2.35) that satisfy the second and third of Eqs. (8.1.16),
q& = Dv& - UBF t
(8.1.23)
&& = Dv
q && - UBg
where
(
D º I - UBF q V ) (8.1.24)
The results of Eqs. (8.1.20) and (8.1.23) may be substituted into the equations of motion of Eq.
(8.1.15), written in residual form as
v, v& , v, λ ) º MDv
R (&& && + F qT λ - MUBg - Q A = 0 (8.1.25)
8.1.2.4 Explicit Integration of the Index 0 DAE

For explicit integration of Eq. (8.1.25), which is linear in &v& and nonlinear in l due the
dependence Q A = Q A ( v, v& , l ,t) , the derivative Q lA is required. The Jacobian of the residual of
Eq. (8.1.25) is
J &&v ,l = éë MD FqT - QlA ùû (8.1.26)
v j and l j , Newton-Raphson
With known values of v and v& at a given time step and estimates &&
iteration is
595
é D&&
vj ù
v j , v& , v, l j )
J &&v ,l ê j ú = - R (&&
ë Dl û
(8.1.27)
é &&
v j+1 ù é && v j ù é D&&
vjù
=
ê j+1 ú ê j ú ê j ú +
ë l û ë l û ë Dl û
v and l is achieved.
Until convergence to &&
An explicit integrator such as Nystrom4 is then used to integrate for v and v& at the next
time step. The process is continued until the desired final time is reached. This explicit method is
implemented in Code 8.1 of Appendix 8.A.
8.1.2.5 Implicit integration of the Index 0 DAE
In order to solve Eq. (8.1.25) using an implicit numerical integration formula, the
following quantities, defined in Sections 5.2 and 5.3, are required:
P2(q, χ ) º ( F q (q, ˆt)χˆ )
q
( )
P3(q, q& ) º P2(q, q&ˆ )qˆ&
q
P4(q, λ ) º ( Φ (q,t)
ˆ λˆ )
q
T
q
M2( q, β ) º ( M(q)βˆ )
(8.1.28)
q
g = P2(q, q& )q&

g q = P3(q, q& )
g q& = 2P2(q, q& )
where χ has the same dimension as q. Using trapezoidal integration formulas of Eq. (4.8.40),
v n = v n -1 + h&v n -1 + (h 2 / 4)(&&v n -1 + V T&&q n )
(8.1.29)
&v n = &v n -1 + (h / 2)(&&v n -1 + V T&&q n )
Derivatives of Eq. (8.1.25) with respect to solution variables &&v and λ are
v = MD + (h/2)R v& + (h 2 /2)R v
dR / d&&
(8.1.30)
R λ = FTq + QλA
where partial derivatives of the residual, in terms of the relations of Eq. (8.1.28), are given in Eq.
(5.5.18) as
&& - MUBP2(q,&&q) - MUBg q + P4(q, λ)
é M 2(q,q) ù
Rv = ê úD
( & + F tq
êë -Sq - Qq + MUBg &q + S&q + Q&q UB P2(q,q)
A A
) ( ) úû (8.1.31)
(
R&v = - MUBg &q + S&q + Q&qA D )
Newton-Raphson iteration to solve Eq. (8.1.25) is carried out at each time step, using
596
éV&&
vi ù
[dR / d&&v R l ] ê ni ú = -R (&&
v in , l ni )
ëVl n û
(8.1.32)
é &&
v i+1 ù é &&
v in ù é V&&
v in ù
= +
ê i+1 ú ê i ú ê i ú , i=1,2,L
n
ë l n û ël n û ëVl n û
Since the tangent space parameterization of Eq. (8.1.20) is valid only locally, criteria
such as the condition number of the coefficient matrix on the left of the first of Eqs. (8.1.32) is
monitored to determine when a new parameterization is required. For more detail on the Index 0
DAE algorithm and its implementation, the reader is referred to Section 5.5.
8.1.3. Simulation of Three Mass planar Mechanism
To implement the tangent space algorithm of Section 5.5 for the mechanism of Section
8.1.2.1, using the continuously differentiable friction model of Section 8.1.2.2, the applied
generalized force of Eq. (8.1.8) and its derivatives are
é - m1g - k1q1 - λq 2 csign(λq 2 )S(q& 1 ) ù
ê ú
Q (q , q& ,λ) = ê k 2 ( q 3 - q 2 - 1) - (λq1 - m 2 g)csign(λq1 - m 2 g)S(q& 2 ) ú
A
êë - k 2 ( q 3 - q 2 - 1) - m 3gS(q& 3 ) úû
é -k1 - ( λcsign(λq 2 ) + l 2q 2dcsign(λq 2 ) ) S(q& 1 ) 0 ù

ê ú
QqA = ê -aS(q& 2 ) -k 2 k2 ú
ê 0 k2 -k 2 úú
êë û
a = λcsign(λq1 - m2 g) + l(λq1 - m 2g)dcsign(λq1 - m 2g)
é - λq 2 csign(λq 2 )S¢(q& 1 ) 0 0 ù
Q =
A ê 0 -(λq1 - m 2 g)csign(λq1 - m 2 g)S¢ (q& 2 ) 0 ú
q&ê ú
êë 0 0 - m 3gS¢(q& 3 ) úû
é - ( q 2 csign(λq 2 ) + lq 22 dcsign(λq 2 ) ) S(q& 1 ) ù

ê ú
Q Aλ = ê - ( q1csign(λq1 - m 2 g) + q1 (λq1 - m 2 g)dcsign(λq1 - m 2 g) ) S(q& 2 ) ú
ê 0 ú
êë úû
ˆ º ( M(q)β ) = 0 . The remaining
Since M of Eq. (8.1.8) does not depend on q, M 2(q,β) q
kinematic derivatives of Eqs. (8.1.28) are P 2(q, χ ) = ( Φ q χˆ ) = [ c1 c 2 0] ,

q
(
P3(q, q& ) = P 2(q, qˆ& )qˆ& )
q
( )
= 0 , and P4(q, l) = ΦqT λˆ
q
= diag(l, l, 0) .
For numerical simulation, model parameters are selected as m1 = 6.3 kg, m2 = m3 = 2 kg,
g = 9.8 m / sec 2 , step size is h = e-4 sec, vt = 20h sec, μ d = 0.3 , μ s = 0.5 , k1 = k 2 = 10 N/m, and
l = 5 m. Initial conditions are q 0 = [ 0 5 6] m and q& = [ 0 0 -1] m/sec. Numerical
T T
597
solutions are obtained using MATLAB Code 8.1 of Appendix 8.A, with the implicit trapezoidal
integration algorithm of Section 5.5.4 and the explicit Nystrom4 integration algorithm of Section
4.8. Plots of the solution in Fig. 8.1.5 show positions q i and velocities &q i º q i d of the three
masses vs time. Essentially identical solutions are obtained with both implicit and explicit
integrators. This shows that this dynamic simulation model with friction is not stiff.
Figure 8.1.5 q i and q& i º q i d vs Time, m1 = 6.3 kg

As shown on the right of Fig. 8.1.5, mass three is stationary (stiction) from approximately
0.25 to 0.85 sec and after 3.3 sec. At approximately 1.5 sec, the velocities of masses one and two
are simultaneously zero, but stiction does not occur. As shown more clearly in the inset of Fig.
8.1.6, q& 2 is zero for a short period and then varies until approximately 1.8 sec. From 1.8 to 2.2
sec, masses one and two are simultaneously stationary; i.e., stiction. This behavior is consistent
with the kinematic velocity constraint Φq q& = q1q& 1 + q 2 q& 2 = 0 . Provided q1 ¹ 0 ¹ q 2 , q& 1 = 0
implies q& 2 = 0 and vice versa. This is the source of the redundant stiction constraint identified by
Wojtyra (2016). Throughout periods of predicted stiction, the variation in slip velocity is less
than e-3 m/sec. At approximately 3.3 sec, the velocities of all three masses are zero, after which
no motion occurs.
Figure 8.1.6 Stiction Interval for m1 = 6.3 kg
To confirm the validity of simulation results, integration step size h and parameter vt are
reduced to e-6 sec and 2e-5 m/sec, respectively. The dissipation of energy due to slip velocity
during the period of approximate stiction of masses one and two is 1.2e-4 N.m, a small fraction
598
of the total frictional loss of 187 N.m over the period of the simulation. This reflects the limit of
v t = 2e-5 m/sec on slip velocity during periods of approximate stiction. Only 20
reparameterizations in 4e6 time steps are required (2e5 time steps per reparameterization).
Further, the equations of motion are satisfied to within an integration tolerance of e-6 and the
maximum norms of error in position, velocity, and acceleration constraints over the simulation
period are 2e-13 , 5e-13 , and 2e-12 , respectively. It has been reported by Marques, Flores,
Claro, and Lankarani (2016) that the Index 3 equations of motion they employ to evaluate a
similar model of friction appear to be stiff. In contrast, the tangent space Index 0 DAE
formulation with explicit Nystrom4 and implicit trapezoidal integration methods succeeded
herein, show that the model is not stiff.
To investigate the sensitivity of stiction behavior to variations in model parameters, the
mass of body 1 is increased to m1 = 6.4 kg and yields the response shown in Fig. 8.1.7. In this
case, velocities of masses one and two go simultaneously to zero at approximately 1.5 sec and, in
contrast to the behavior shown in Figs. 8.1.5 and 8.1.6, stiction of masses one and two occurs
and persists until approximately 2.2 sec.
Figure 8.1.7 q i and q& i º q i d vs Time, m1 = 6.4 kg

A parametric study confirmed the characteristic delay in onset of stiction shown in Figs.
8.1.5 and 8.1.6. With mass one increased from 8.3 kg to 8.3466602 kg, the magnitude of delay in
onset of stiction shown in Figs 8.1.5 and 8.1.6 and intervening slip velocities are essentially
invariant. With mass one equal to 8.3466603 kg, a 1.5e-6 % increase, the instant initiation of
stiction shown in Fig. 8.1.7 occurs. A similar change in qualitative behavior occurred due to
variations in other model parameters.
The Index 0 DAE for dynamic systems with no friction are shown in Chapter 5 to be
equivalent to ODE, hence well posed; i.e., they have unique solutions that depend continuously
on problem data. It has been shown, under more restrictive assumptions (Blumentals, Brogliato,
and Bertails-Descoubes, 2016), that systems under the influence of friction forces with nonzero
slip velocities; i.e., without stiction, are well behaved. The foregoing example, however, has
solutions that depend discontinuously on data. The result is a clear indicator that multibody
dynamic response in the presence of stiction may be a discontinuous function of model
parameters. Thus, problems of multibody dynamics with friction are not well posed. It is
interesting to note that this discontinuous behavior was reported (Haug, 2017c) with the
599
transition value of m1 that was 1.2% higher than reported here. The difference is due to use of the
continuously differentiable absolute value function here, whereas the basic absolute value
function was used in the reference. Using the explicit Nystrom4 integrator with the algorithm of
Section 5.5.2, which is implemented in Code 8.1 of Appendix 8.1, the transition occurs with a
value of m1 that is within 0.007% of that reported above. This is significant for two reasons.
First, the explicit integrator is more sensitive to discontinuous derivatives than the trapezoidal
integrator. Second, and more important, if there was stiffness in the model, the explicit integrator
would have failed, which it did not. This supports the contention that models with friction are not
necessarily stiff.
A partial explanation of the discontinuous dependence of dynamic response on model
parameters is had by studying the effective coefficients of friction between sliding surfaces and
masses one and two in the vicinity of a stiction event. Define the effective coefficient of friction
as the friction force that is predicted by the continuous friction model divided by the associated
normal force. Effective coefficients of friction obtained from simulation of the planar three mass
model problem are plotted in Fig. 8.1.8. As shown in the upper plot with m1 = 6.4 kg, when
sliding velocities of the bodies are simultaneously zero at approximately 1.58 sec, both
coefficients of friction rise from md to ms , but stiction is not achieved. Both masses continue to
slide until just after 1.8 sec, when both effective coefficients of friction simultaneously reach the
value ms . At this point, stiction occurs and continues until approximately 2.17 sec. At this time,
both effective coefficients of friction again rise to ms and the stiction bond is broken, leading to
sliding with an effective coefficient of friction md . In contrast, as shown in the lower plot of Fig.
8.1.8 with m1 = 6.5 kg, the effective coefficients of friction for both bodies simultaneously reach
the value ms at approximately 1.58 sec, after which stiction is initiated and continues until
approximately 2.17 sec. As with the prior case, both effective coefficients of friction
simultaneously achieve the value ms at 2.17 sec, the stiction bond is broken, and the masses slide
with nonzero slip velocity. This is consistent with the slip velocity behavior shown in Figs. 8.1.5,
8.1.6, and 8.1.7. This behavior appears to be too systematic to be an accident. It is reminiscent of
Newton’s First Law, as noted in Section 4.1.1; i.e., bodies at rest tend to stay at rest (stiction),
until external effects lead to motion.
600
Figure 8.1.8 Effective Coefficients of Friction for Masses One and Two
The tangent space Index 0 DAE formulation, with both implicit trapezoidal and explicit
Nystrom4 numerical integration algorithms, yields reliable results in the three mass model
problem and shows no sign of stiffness, in contrast to stiffness reported in the literature with
Index 3 DAE formulations. These results demonstrate the potential for use of the continuously
differentiable friction approximation approach with the tangent space Index 0 DAE
formulation for modeling and simulation of friction and stiction in multibody dynamics.
A theoretically interesting point is that the equations of motion with friction, even with the
continuous model used herein, are not well posed.
Key Formulas
Ff = - Fn (q, λ )S(v,μ ) (8.1.9)
S(v,μ ) = m d tanh(4v / v t ) + (m s - m d )(v / v t ) / ( (v / 2v t )2 + 3 / 4 )

2
(8.1.10)
&& + F q (q )T l - Q A (q, q& , λ ) = 0

M(q )q (8.1.15)
F (q) = 0
F qq& = 0 (8.1.16)
F q&& ( )
q + F q qˆ& q& º F q&&
q
q+g =0
601
8.2 Analytical Stiction Criteria and Ranges of Stiction Forces
Inequalities that characterize discontinuous Coulomb friction lead to complex logic that
defines criteria for the onset of static friction, or stiction. These inequalities are studied in this
section for the three-mass model problem, leading to criteria that, with considerable effort, yield
a general solution that defines admissible ranges of stiction forces. A numerical solution is
presented for the model problem and is shown to agree with the continuous model introduced in
Section 8.1. The complexity of the equations encountered for even this simple model problem
with discontinuous Coulomb friction suggests that analytical solutions of more realistic
applications are unlikely. The reader who is primarily interested in numerical methods for
simulation of systems with friction may wish to bypass this section, or to review the results
without suffering all the details.
8.2.1 Analytical Stiction Criteria for the Three Mass Model Problem
In order to determine whether stiction will occur during a finite time interval, beyond a
time t at which q& 1 ( t ) = q& 2 ( t ) = 0 , the conditions &&
q1 (t) = &&
q 2 (t) = 0 , q1 (t) = q1 , and q2 (t) = q2 are
imposed during the interval. Stiction forces sf1 and sf 2 are presumed to act in the negative
coordinate directions on masses one and two in Fig. 8.1.2, replacing the sliding friction forces on
masses one and two in Eq. (8.1.8). The first two equations of motion of Eq. (8.1.8) at t = t thus
reduce to
sf1 = -λq1 - m1g - k1q1
(8.2.1)
sf 2 = -λq 2 + k 2 ( q 3 - q 2 - 1)
where the over bar is suppressed for notational convenience. Necessary and sufficient conditions
for static equilibrium; i.e., stiction, to occur during the subsequent time interval with a static
coefficient of friction ms are that the Coulomb friction inequalities sfi £ ms Ni , i=1,2 , are
satisfied. Using Eqs. (8.1.7) and (8.2.1), this is
-λq1 - m1g - k1q1 £ -ms λq 2
(8.2.2)
-λq 2 + k 2 ( q 3 - q 2 - 1) £ ms ( m 2 g - λq1 )
It is interesting to note that the same criteria may be obtained by imposing nonholonomic
constraints that require q& 1 (t) = q& 2 (t) = 0 , with associated Lagrange multipliers playing the role
of sf1 and sf 2 in Eq. (8.2.1) (Wojtyra, 2016).
With q1 , q2 , and q3 evaluated at t , the inequalities of Eqss. (8.2.2) are functions of λ . If

the set of λ that satisfy both inequalities of Eq. (8.2.2) is nonempty, then stiction can occur
during a subsequent interval of time. If no value of λ satisfies Eqs. (8.2.2), stiction cannot occur
and the equations of motion can be integrated beyond t . If stiction does occur, there are
generally many values of the Lagrange multiplier that yield the same solution of the constrained
equations of motion. This is consistent with the Coulomb friction law that is set valued during
stiction.
The following absolute value inequalities for scalar functions a( l ) and b( l ) are key to
defining analytical criteria for stiction:
602
a(l ) £ b(l ) Û a(l ) £ b(l ) and - a(l ) £ b(l )
a(l ) £ b(l ) Û a(l ) £ b(l ) or b(l ) £ -a(l ) (8.2.3)
-a(l ) £ b(l ) Û - a(l ) £ b(l ) or b(l) £ a(l )
In terms of sets
A(l ) º {l : a(l ) £ b(l ) }
D(l ) º { l : a(l ) £ b(l )}
E(l ) º {l : b(l ) £ -a(l )} (8.2.4)
F(l ) º {λ : -a(l ) £ b(l )}
G(l ) º {l : b(l ) £ a(l )}
the set of solutions of the inequalities of Eq. (8.2.3) is
A(l) = ( D(l) È E(l) ) Ç ( F(l) È G(l) ) (8.2.5)
where È and Ç denote union and intersection of sets, respectively. Let A1 (λ) and A2 (λ) be sets
of values of λ that satisfy the first and second inequalities of Eqs. (8.2.2), respectively, at a time
t for which q& 1 ( t ) = q& 2 ( t ) = 0 . If A1 (λ) Ç A2 (λ) is nonempty, both inequalities of Eqs. (8.2.2)
are satisfied for each λ in A1 (λ) Ç A2 (λ) and stiction can occur in a subsequent interval of time.
Conversely, if A1 (λ) Ç A2 (λ) is empty, one or both of the inequalities in Eqs. (8.2.2) is not
satisfied for any λ , and stiction cannot occur in a subsequent interval of time.
In the present example, for the first inequality of Eqs. (8.2.2),
D1 (λ) = {λ : λ ( m s q 2 - q1 ) £ m1g + k1q1} = {λ : λ ( a ) £ g}
E1 (λ) = {λ : λ ( -ms q 2 - q1 ) £ m1g + k1q1 } = {λ : λ ( -b ) £ g}
(8.2.6)
F1 (λ) = {λ : λ ( q1 + ms q 2 ) £ -m1g - k1q1 } = {λ : λ ( b ) £ -g}
G1 (λ) = {λ : λ ( q1 - ms q 2 ) £ -m1g - k1q1} = {λ : λ ( -a ) £ -g}
where a = μ sq 2 - q1 , b = μ sq 2 + q1 , and g = m1g + k1q1 . For the second inequality,
D 2 (λ) = {λ : - λ ( q 2 + ms q1 ) £ - k 2 ( q 3 - q 2 - 1) - ms m 2 g} = {λ : λ ( e ) £ -t}
E 2 (λ) = {λ : λ ( ms q1 - q 2 ) £ - k 2 ( q3 - q 2 - 1) + ms m 2g} = {λ : λ ( -d ) £ -h}
(8.2.7)
F2 (λ) = {λ : λ ( q 2 - ms q1 ) £ k 2 ( q3 - q 2 - 1) - ms m 2g} = {λ : λ ( d ) £ h}
G 2 (λ) = {λ : λ ( ms q1 + q 2 ) £ k 2 ( q 3 - q 2 - 1) + ms m 2g} = {λ : λ ( -e ) £ t}
where d = μ s q1 + q 2 , e = μ s q1 - q 2 , h = k 2 ( q 3 - q 2 - 1) + μ s m 2g , and t = k 2 ( q3 - q 2 - 1) - μ s m 2g .
603
8.2.2 Analytical Solution of Stiction Inequalities
Solutions of the linear inequalities on the right of Eqs. (8.2.6) and (8.2.7) may be
tabulated and the sets of solutions A1 (l) and A2 (l) of Eq. (8.2.5) and their intersection may be
evaluated, to establish criteria for the occurrence of stiction. Solutions based on the algebraic
signs of parameters a and b in Eq. (8.2.6) and d and e in Eqs. (8.2.7) are evaluated in the
following.
For Eqs. (8.2.5) and (8.2.6);
If a > 0 and b < 0 , D1 (λ) = {λ : λ £ g / a} , E1 (λ) = {λ : λ £ - g / b} , F1 (λ) = {λ : λ ³ - g / b} ,
G1 (λ) = {λ : λ ³ g / a} , D1 (λ) È E1 (λ) = {λ : λ £ max ( g / a, - g / b )} ,
F1 (λ) È G1 (λ) = { λ : λ ³ min ( g / a , - g / b )} , and
A1 (λ) = {λ : min ( g / a , - g / b ) £ λ £ max ( g / a , - g / b )} (8.2.8)
If a > 0 and b > 0 , D1 (λ) = { λ : λ £ g / a} , E1 (λ) = { λ : λ ³ - g / b} , F1 (λ) = { λ : λ £ - g / b} ,
G1 (λ) = { λ : λ ³ g / a} , D1 (λ) È E1 (λ) = { λ : λ £ g / a or λ ³ - g / b} ,
F1 (λ) È G1 (λ) = { λ : λ £ - g / b or λ ³ - g / a} , and
A1 (λ) = { λ : λ £ min ( g / a , - g / b ) or λ ³ max ( g / a , - g / b )} (8.2.9)
If a < 0 and b > 0 , D1 (λ) = {λ : λ ³ g / a} , E1 (λ) = {λ : λ ³ - g / b}, F1 (λ) = {λ : λ £ - g / b} ,

G1 (λ) = {λ : λ £ g / a} , D1 (λ) È E1 (λ) = {λ : λ ³ min ( g / a, - g / b )} ,
F1 (λ) È G1 (λ) = { λ : λ £ max ( g / a , - g / b )} , and
A1 (λ) = {λ : min ( g / a , - g / b ) £ λ £ max ( g / a , - g / b )} (8.2.10)
If a < 0 and b < 0 , D1 (λ) = {λ : λ ³ g / a} , E1 (λ) = {λ : λ £ - g / b}, F1 (λ) = {λ : λ ³ - g / b} ,

G1 (λ) = {λ : λ £ g / a} , D1 (λ) È E1 (λ) = {λ : λ £ - g / b or λ ³ g / a} ,
F1 (λ) È G1 (λ) = { λ : λ £ g / a or λ ³ - g / d} , and
A1 (λ) = { λ : λ £ min ( g / a , - g / b ) or λ ³ max ( g / a , - g / b )} (8.2.11)
In summary, apart from cases with a = 0 or b = 0 ,
A1 (λ) = {λ : min ( g / a, - g / b ) £ λ £ max ( g / a, - g / b )} , if ab < 0

(8.2.12)
A1 (λ) = {λ : λ £ min ( g / a, - g / b ) or λ ³ max ( g / a, - g / b )} , if ab > 0
For Eqs. (8.2.5) and (8.2.7);

If d > 0 and e < 0 , D 2 (λ) = {λ : λ ³ - t / e} , E 2 (λ) = {λ : λ ³ h / d}, F2 (λ) = {λ : λ £ h / d} ,
G 2 (λ) = {λ : λ £ - t / e}, D2 (λ) È E 2 (λ) = {λ : λ ³ min ( h / d, - t / e )} ,
F2 (λ) È G 2 (λ) = {λ : λ £ max ( h / d, - t / e )} , and
604
A 2 (λ) = {λ : min ( h / d, - t / e ) £ λ £ max ( h / d, - t / e )} (8.2.13)
If d > 0 and e > 0 , D 2 (λ) = {λ : λ £ - t / e} , E 2 (λ) = {λ : λ ³ h / d}, F2 (λ) = {λ : λ £ h / d} ,

G 2 (λ) = {λ : λ ³ - t / e}, D2 (λ) È E 2 (λ) = {λ : λ £ - t / e or λ ³ h / d} ,
F2 (λ) È G 2 (λ) = { λ : λ £ h / d or λ ³ - t / e} , and
A 2 (λ) = {λ : λ £ min ( h / d, - t / e ) or λ ³ max ( h / d, - t / e )} (8.2.14)
If d < 0 and e > 0 , D 2 (λ) = {λ : λ £ - t / e} , E 2 (λ) = {λ : λ £ h / d}, F2 (λ) = {λ : λ ³ h / d} ,

G 2 (λ) = {λ : λ ³ - t / e} , D 2 (λ) È E2 (λ) = {λ : λ £ max ( h / d, - t / e )} ,
F2 (λ) È G 2 (λ) = { λ : λ ³ min ( h / d, - t / e )} , and
A 2 (λ) = {λ : min ( h / d, - t / e ) £ λ £ max ( h / d, - t / e )} (8.2.15)
If d < 0 and e < 0 , D 2 (λ) = {λ : λ ³ - t / e} , E 2 (λ) = {λ : λ £ h / d}, F2 (λ) = {λ : λ ³ h / d} ,

G 2 (λ) = {λ : λ £ - t / e}, D2 (λ) È E 2 (λ) = {λ : λ £ h / d or λ ³ - t / e} ,
F2 (λ) È G 2 (λ) = { λ : λ £ - t / e or λ ³ h / d} , and
A 2 (λ) = {λ : λ £ min ( h / d, - t / e ) or λ ³ max ( h / d, - t / e )} (8.2.16)
In summary, apart from cases with d = 0 or e = 0 ,

A 2 (λ) = {λ : min ( h / d, - t / e ) £ λ £ max ( h / d, - t / e )} , if de < 0
(8.2.17)
A 2 (λ) = {λ : λ £ min ( h / d, - t / e ) or λ ³ max ( h / d, - t / e )} , if de > 0
Apart from zero values of a, b, d, or e that influence only potentially infinite ranges of
admissible λ , Eqs. (8.2.12) and (8.2.17) yield four possible cases. Defining
min1 = min ( g / a, - g / b ) , max1 = max ( g / a, - g / b ) , min2 = min ( h / d, - t / e ) , and
max 2 = max ( h / d, - t / e ) , solutions for λ in the four cases shown in Fig. 8.2.1 are as follows:
(1) If ab < 0 and de < 0 (Fig. 8.2.1(1)),
ì f, iff max(min1,min2) > min(max1,max2)

A1 (λ) Ç A 2 (λ) = í (8.2.18)
î{ λ : max(min1,min2) £ λ £ min(max1,max2} , otherwise
where f denotes the null, or empty, set and iff reads “if and only if”.
(2) If ab > 0 and de < 0 (Fig. 8.2.1(2)),
ì f, iff min2 > min1 and max2 < max1

A1 (λ) Ç A 2 (λ) = í (8.2.19)
î{ λ : min 2 £ λ £ min1 or max1 £ λ £ max 2} , otherwise
(3) If ab < 0 and de > 0 (Fig. 8.2.1(3)),
605
ì f, iff min2 < min1 and max2 > max1
A1 (λ) Ç A 2 (λ) = í (8.2.20)
î{ λ : min1 £ λ £ min 2 or max2 £ λ £ max1} , otherwise
(4) If ab > 0 and de > 0 (Fig. 8.2.1(4)),
A1 (λ) Ç A 2 (λ) = { λ : λ £ min ( min 1, min 2 ) or λ ³ max ( max1,max2 )} ¹ f (8.2.21)
Figure 8.2.1 Admissible Subsets of A1 (λ) and A 2 (λ) as Dark Lines

8.2.3 Admissible Ranges of Stiction Forces
Admissible ranges of stiction forces for the three mass example are determined by Eq.
(8.2.1) and the ranges of values of Lagrange multipliers defined in Eqs. (8.2.18) to (8.2.21), as
follows:
(1) If max(min1,min2) £ min(max1,max2) in Eq. (8.2.18), bounds on stiction forces that are
specified in Eqs. (8.2.1) are
ì - min(max1,max2)q1 - m1g - k1q1 £ sf1 £ - max(min1,min2)q1 - m1g - k1q1 , if q1 ³ 0 ü
í ý
î - max(min1,min2)q1 - m1g - k1q1 £ sf1 £ - min(max1,max2)q1 - m1g - k1q1 , if q1 < 0 þ
ìï - min(max1,max2)q 2 + k 2 ( q3 - q 2 - 1) £ sf 2 £ - max(min1,min2)q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 ³ 0 üï
í ý
ïî - max(min1,min2)q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - min(max1,max2)q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 < 0 ïþ
(8.2.22)
(2) If min2 £ min1 in Eq. (8.2.19), bounds on stiction forces that are specified in Eqs. (8.2.1) are
ì - min1q1 - m1g - k1q1 £ sf1 £ - min2q1 - m1g - k1q1 , if q1 ³ 0 ü
í ý
î - min2q1 - m1g - k1q1 £ sf1 £ - min1q1 - m1g - k1q1 , if q1 < 0 þ
(8.2.23)
ìï - min1q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - min2q2 + k 2 ( q 3 - q 2 - 1) , if q 2 ³ 0üï
í ý
îï - min2q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - min1q2 + k 2 ( q 3 - q 2 - 1) , if q 2 < 0ïþ
or, if max2 ³ max1 in Eq. (8.2.19), bounds on stiction forces that are specified in Eqs. (8.2.1) are
606
ì - max2q1 - m1g - k1q1 £ sf1 £ - max1q1 - m1g - k1q1 , if q1 ³ 0 ü
í ý
î - max1q1 - m1g - k1q1 £ sf1 £ - max2q1 - m1g - k1q1 , if q1 < 0 þ
(8.2.24)
ïì - max2q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - max1q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 ³ 0ïü
í ý
îï - max1q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - max2q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 < 0ïþ
(3) If min2 ³ min1 in Eq. (8.2.20), bounds on stiction forces that are specified in Eqs. (8.2.1) are
ì - min2q1 - m1g - k1q1 £ sf1 £ - min1q1 - m1g - k1q1 , if q1 ³ 0 ü
í ý
î - min1q1 - m1g - k1q1 £ sf1 £ - min2q1 - m1g - k1q1 , if q1 < 0 þ
(8.2.25)
ïì - min2q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - min1q2 + k 2 ( q 3 - q 2 - 1) , if q 2 ³ 0üï
í ý
îï - min1q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - min2q2 + k 2 ( q 3 - q 2 - 1) , if q 2 < 0ïþ
or, if max2 £ max1 in Eq. (8.2.20), bounds on stiction forces that are specified in Eqs. (8.2.1)
are
ì - max1q1 - m1g - k1q1 £ sf1 £ - max2q1 - m1g - k1q1 , if q1 ³ 0 ü
í ý
î - max2q1 - m1g - k1q1 £ sf1 £ - max1q1 - m1g - k1q1 , if q1 < 0 þ
(8.2.26)
ïì - max1q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - max2q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 ³ 0ïü
í ý
îï - max2q 2 + k 2 ( q 3 - q 2 - 1) £ sf 2 £ - max1q 2 + k 2 ( q 3 - q 2 - 1) , if q 2 < 0ïþ
(4) Ranges of stiction forces, as a result of Eq. (8.2.21) and Eqs. (8.2.1), are
ïì - min ( min1, min 2 ) q1 - m1g - k1q1 £ sf1 , if q1 ³ 0 üï

í ý
ïîsf1 £ - min ( min1, min 2 ) q1 - m1g - k1q, if q1 < 0 ïþ
(8.2.27)
ìï - min ( min1, min 2 ) q 2 + k 2 ( q3 - q 2 - 1) £ sf 2 , if q 2 ³ 0 üï
í ý
ïîsf 2 £ - min ( min1, min 2 ) q 2 + k 2 ( q3 - q 2 - 1) , if q 2 < 0 ïþ
or,
ïìsf1 £ - max ( max1, max 2 ) q1 - m1g - k1q1 , if q1 ³ 0 üï

í ý
îï - max ( max1, max 2 ) q1 - m1g - k1q £ sf1 , if q1 < 0 þï
(8.2.28)
ìïsf 2 £ - max ( max1, max 2 ) q 2 + k 2 ( q3 - q 2 - 1) , if q 2 ³ 0 üï
í ý
îï - max ( max1, max 2 ) q 2 + k 2 ( q3 - q 2 - 1) £ sf 2 , if q 2 < 0 þï
Since conditions for occurrence of stiction in Eqs. (8.2.18) through (8.2.21) depend only
on problem data and generalized coordinates in solutions of the equations of motion, they are
only necessary conditions for stiction of masses one and two to occur. They are not sufficient
conditions for onset of stiction, since slip velocities must be zero in order for stiction to follow.
Once stiction occurs, any stiction force in the ranges defined in Eqs. (8.2.22) through (8.2.28) is
equivalent for stiction of both masses .
607
To determine the solution for generalized coordinates in the three mass model problem of
Section 8.1.2, a constraint may be imposed on the position of either mass one or mass two during
a stiction period involving these two masses, for solution of the equations of motion. The
resulting generalized coordinates must be identical for either choice of a single stiction
constraint, since enforcing a position constraint on either mass imposes a constraint on the
position of the other mass, due to the holonomic constraint of Eq. (8.2.5). Lagrange multipliers
determined in the alternative solutions will be different and will generally not yield stiction
forces on the individual masses that satisfy Eqs. (8.2.2). However, stiction forces determined by
Eqs. (8.2.22) through (8.2.28), using results from either solution, will satisfy Eqs. (8.2.2).
Treatment of a stiction event that involves mass three is much simpler than for masses
one and two, since it is not subject to a kinematic constraint. A stiction constraint
q3 (t) - q3 (t) = 0 is imposed for t > t if q& 3 ( t) = 0 and the associated Lagrange multiplier λ3 (t) is
the stiction force that must satisfy the condition λ3 (t) £ μ s m3g for continuation of stiction, after
which the stiction constraint is deleted.
8.2.4 Numerical Solution of Coulomb Stiction Criteria
Equations of motion for the example of Section 8.1.2, with the same data, are
implemented with stiction constraints on first mass one and then mass two. Simulations are
carried out using MATLAB Code 8.2 in Appendix 8.A, with implicit trapezoidal integration and
the tangent space Index 0 DAE formulation with m1 = 5 and 7 kg, h = e-4 , other data as in
Section 8.1.2, and a tolerance tol = e-3 on the conditions max( q& 1 , q& 2 ) £ tol and q& 3 £ tol for
onset of stiction for masses one and two and for mass three, respectively. Results with a
holonomic stiction constraint on mass one or two yield identical solutions for generalized
coordinate velocities shown in Fig. 8.2.2.
Figure 8.2.2 q& i (qi d) vs. Time for m1 = 5 and 7

Generalized coordinate histories are similar to those presented in Figs. 8.1.5 and 8.1.7.
Only 20 reparameterizations were required in 40,000 time steps (2000 time steps per
reparameterization), and there was no sign of stiffness in the tangent space equations of motion.
To compare solutions based on the continuous Coulomb friction model presented in Section
8.1.2.2 with the constraint addition-deletion analysis of this section, norms of differences in
solutions for m1 = 8.5 are evaluated. The mean norms of differences in positions, velocities, and
accelerations over 40,000 time steps are 0.0042, 0.0054, and 0.159, respectively.
608
The large range of values of m1 chosen for the results of Fig 8.2.2 is selected to enable
clear graphical explanation of the theoretical basis for the difference in stiction response modes.
Based on the criteria of Eqs. (8.2.18) through (8.2.21), stict = 0 if stiction cannot occur in the
present state of the mechanism and stict = 1 if stiction can occur; i.e., the necessary conditions
are satisfied. If the necessary conditions for stiction are satisfied and the slip velocities are zero
at a given time, stiction occurs and mode = 2 holds until the necessary conditions fail to be
satisfied, at which time the value of stict is set to 0 and mode is set to 1. Values of these
functions are shown in Fig. 8.2.3.
Figure 8.2.3 Triggers for Onset and Termination of Stiction Event

As shown in the top plot of Fig. 8.2.3, for m1 = 5 , the necessary conditions for stiction
are not satisfied and stict = 0 at 1.58 sec when the slip velocities go to zero. Thus, the state of the
system precludes the onset of stiction, and nonzero slip velocities persist until just before 2 sec,
when they go to zero. At this time, the necessary conditions are satisfied and stict = 1. Therefore,
stiction is initiated and continues until approximately 2.2 sec, at which time the necessary
conditions for stiction fail to be satisfied and sliding resumes. In contrast to stiction behavior for
m1 = 5 , with m1 = 7 , the lower plot of Fig. 8.2.3 shows that the necessary conditions for stiction
to occur are satisfied and stict = 1 when the slip velocities first go to zero. Stiction is thus
initiated and continues until 2.2 sec, when it is terminated, as in the preceding case.
609
Maximum and minimum limits on stiction forces that act on masses one and two, from
Eqs. (8.2.22) through (8.2.28) that satisfy Eq. (8.2.2), are obtained using generalized coordinates
from individual mass stiction simulations and are shown in Fig. 8.2.4. These plots show the set-
valued nature of Coulomb friction force, with any stiction force values between the maximum
and minimum bounds satisfying Eq. (8.2.2) and yielding the same solution of the equations of
motion for generalized coordinates. Note that, at the beginning and end of stiction periods, both
inequalities of Eqs. (8.2.2) are equalities, as shown in simulations in Section 8.1.2.3.
Figure 8.2.4 Coulomb Stiction Force Range on Masses 1 and 2

It is not clear how to extend the foregoing analytical criteria to general multibody
systems. However, analysis presented for the three mass planar model problem suggests that
simulation using the tangent space Index 0 DAE formulation with the continuous friction model
and the trapezoidal integration method presented in Section 8.1 provide meaningful results for
analysis of more realistic multibody systems with friction, including stiction events.
Analysis of the three-mass model problem of Sections 8.1 and 8.2 shows that friction and
stiction models using a continuous friction model and an analytical Coulomb friction model
that accounts for stiction effects yield comparable results.
610
8.3 Cartesian Coordinate Simulation of Model Problems
As a first step in extension of the analysis presented in Section 8.1 to general multibody
systems, which involves detailed models of constraint geometry, constraint reaction forces, and
friction forces, Cartesian coordinate models of the three mass system of Sections 8.1 and 8.2 and
a four mass spatial system with a higher degree of stiction redundancy are studied.
8.3.1 Cartesian Coordinate Simulation of a Three Mass System
For the planar three mass model problem of Fig. 8.1.2, vectors r1 = [ x 1 y1 ] and
T
r2 = [ x 2 y 2 ] that locate masses one and two and the x3 coordinate of mass three are used as
T
T
Cartesian coordinates; i.e., q = éë x1 y1 x2 y2 x 3 ùû Î R 5 . Constraints acting on masses one
and two and the associated Jacobian are expressed in terms of these coordinates as
é q1 ù
ê ú
F (q) = ê q4 ú=0
ê ú
êë( (q 3 - q1 ) + (q 4 - q 2 ) - l ) / 2 úû
2 2 2
(8.3.1)
é 1 0 0 0 0ù
ê
F q (q ) = ê 0 0 0 1 0 úú
êë q1 - q 3 q 2 - q 4 q 3 - q1 q 4 - q 2 0 úû
Constraint forces between masses one and two and the axes along which they move are Lagrange
multipliers l1 and l2 associated with the first two constraints of Eq. (8.3.1), so system
generalized forces are
é 0 ù
ê - m g - k q - l csign(l )S(q& ) ú
ê 1 1 2 1 1 2 ú
QA (q, q& , λ ) = ê k 2 (q5 - x3 - 1) - l2csign(l 2 )S(q& 3 ) ú (8.3.2)
ê ú
ê - m2g ú
êë - k 2 (q5 - x3 - 1) - m3gS(q& 5 ) úû
With M = diag ( m1 m1 m2 m2 m3 ) , the equations of motion are
&& + FqT (q)λ = QA (q, q& , λ)

Mq (8.3.3)
While the same stiction behavior observed with the reduced dimension model of Sections
8.1 and 8.2 should occur with this model, the analytical approach used in Section 8.2 is not
applicable. The Cartesian coordinate model has three Lagrange multipliers associated with
holonomic constraints, whereas the reduced dimension model used in Section 8.2 has only one.
This special case allowed stiction condition inequalities to be solved as functions of a single
variable. The goal here is to apply the continuous friction model to the higher dimensional
Cartesian coordinate formulation for numerical simulation and compare results with previous
simulations.
611
Quantities that are required to implement the tangent space Index 0 DAE algorithm are
M 2(q , x ) = 0 , P 3(q , q& ) = 0 , and
é 0 2´ 5 ù
(
P 2(q , χ ) = F q (q ) χˆ ) =ê
0 úû
q
ë c1 - c3 c2 - c4 c 3 - c1 c 4 - c2
é l3 0 - l3 0 0ù
ê 0 l3 0 - l3 0ú
ê ú
(
P4(q, λ ) = F q (q)T λˆ ) q
= ê - l3 0
ê
l3 0 0ú
ú
ê 0 - l3 0 l3 0ú
êë 01´5 úû
é 01´5 ù
ê0 - k 0 0 0 úú
ê 1
Qq = ê 0 0 - k 2
A
0 k2 ú
ê ú
ê 01´5 ú
êë0 0 k2 0 - k 2 úû
é 01´5 ù
ê0 l csign(l )S¢ (q& ) 0 0 0 ú
ê 1 1 2 ú
ê
Qq& = - 0
A
0 ¢
l 2csign(l 2 )S (q3 ) 0
& 0 ú
ê ú
ê 01´5 ú
êë0 0 0 0 m3gS¢ (q& 5 ) úû
é 01´3 ù
ê csign(l ) + l dcsign(l ) S(q& ) 0 úú
( 1 ) 0
Qλ = - ê
A 1 1 2
ê 0 ( csign(l 2 ) + l 2dcsign(l 2 ) ) S(q& 3 ) 0 ú

ê ú
ë 02´3 û
Simulations carried out with this model in MATLAB Code 8.3.1 of Appendix 8.A, with
m 2 = m3 = 2 kg , l = 5 m , md = 0.3 , ms = 0.5 , g = 9.8 m/s, Maxvnorm = 0.7, utol = Btol = e-8,
intol = e-6, and varying values of m1 give results that are qualitatively like those of Section 8.1.
Numerical results using implicit trapezoidal and explicit Nystrom4 integrators with the
formulations presented in Section 8.1 are essentially identical. Even the sensitivity of the
discontinuous response event to variations in m1 differs by only 0.01% with the two integrators.
Since the most practical indicator of stiffness in a model is failure of an explicit integrator to
carry out simulations, this system is definitely not stiff. Even with the increased dimension of the
Index 0 DAE of motion, only twenty reparameterizations were required in 4e4 time steps (2,000
time steps per reparameterization) for both implicit and explicit integrators. This reinforces the
effectiveness of the continuous friction model and tangent space trapezoidal integrator in
approximating the solution.
612
8.3.2. Cartesian Coordinate Simulation of a Four Mass System
As a second example of friction and stiction simulation using a Cartesian coordinate
model, consider the four-mass spatial system shown in Fig. 8.3.1. As in the three mass model
problem of Section 8.3.1, stiction will yield redundancy in this model. To see this, note that if the
velocity of mass one is zero, so will be the velocity of mass two, due to the distance constraint
between them. In turn, the velocity of mass 3 will be zero, due to the distance constraint between
it and mass two. This yields a third order constraint reaction force redundancy during stiction.
The analytical approach used in Section 8.2 would be defeated for two reasons; (1) there are
eight Lagrange multipliers, rather than one, and (2) three absolute value inequalities must be
enforced.
Figure 8.3.1 Four Mass Spatial Model Problem

To illustrate the effect of constraint geometry on system dynamics, two contact
geometries between masses and coordinate axis guides are modeled. The rectangular guide along
each axis involves two orthogonal reaction forces, as shown at the upper left in Fig. 8.3.1,
defined by Lagrange multipliers l1 and l2 for axis k. The cylindrical guide shown at the lower
k k
left of Fig 8.3.1, in contrast, involves line contact between the masses and cylinders along which
they move. The resulting contact force, shown at the lower left in Fig. 8.3.1, is the vector sum of
the components acting on cylindrical guide k, so its magnitude is l ik 2 + l kj 2 .
Generalized coordinates for this model are 3-vectors ri , i = 1, 2, 3, that locate masses one
through three and the x coordinate of mass 4, x4 . This is a set of 10 generalized coordinates,
q 9 ùû , and x4 = q10 . With the three-
T T T
with r1 = éëq1 q2 q 3 ùû , r2 = éëq 4 q5 q 6 ùû , r3 = éëq 7 q8
dimensional identity matrix I3 , the system mass matrix is M = diag ( m1I3 m2I3 m3I3 m4 ) .
Holonomic constraints are
613
é q2 ù
ê q3 ú
ê ú
ê q4 ú
ê ú
ê q6 ú
F(q) = ê q7 ú=0 (8.3.4)
ê ú
ê q8 ú
ê ú
ê ( (q 6 - q 3 ) + (q5 - q 2 ) + (q 4 - q1 ) - l1 ) / 2 ú
2 2 2 2
ê ú
ëê( (q 9 - q 6 ) + (q8 - q5 ) + (q 7 - q 4 ) - l 2 ) / 2ûú
2 2 2 2
Using the rectangular guide model, the Lagrange multipliers corresponding to the first six
constraints of Eq. (8.3.4) are constraint reaction forces acting on faces of the rectangular guides
shown in Fig. 8.3.1 and generalized forces acting on the system are
é k 4 (q10 - q1 - 1) - k1q1 - ( l1csign(l1 ) + l 2 csign(l 2 ) ) S(q& 1 ) ù
ê ú
ê 0 ú
ê - m1g ú
ê ú
ê 0 ú
ê - k 2 q5 - ( l3 csign(l 3 ) + l 4 csign(l 4 ) ) S(q 5 )
& ú
Q A (q, q& , λ ) = ê ú (8.3.5)
ê - m2g ú
ê 0 ú
ê ú
ê 0 ú
ê ú
ê - m3g - k 3q 9 - ( l 5csign(l 5 ) + l 6 csign(l 6 ) ) S(q& 9 ) ú
êë - k 4 (q10 - q1 - 1) - m 4 gS(q& 10 ) úû
The remaining task in implementing the tangent space Index 0 DAE formulation with the
trapezoidal integrator is to calculate derivatives of constraint and generalized force related terms.
Since the resulting matrices are even more sparse than their counterparts in Section 8.3.1, only
nonzero terms are presented. Nonzero entries ai j in the ij positions in the constraint Jacobian
Fq (q) are a12 = 1, a23 = 1, a34 =1, a46 =1, a57 =1, a68 =1, a71 = q1 - q4 , a72 = q2 - q5 ,
a73 = q3 - q6 , a74 = q4 - q1 , a75 = q5 - q2 , a76 = q6 - q3 , a84 = q4 - q7 , a85 = q5 - q8 ,
a86 = q6 - q9 , a87 = q7 - q4 , a88 = q8 - q5 , and a89 = q9 - q6 . Nonzero entries bij in the ij positions
of P 2( q , χ ) º ( F q ( q ) χ ) are b71 = c1 - c4, b72 = c2 - c5 , b73 = c3 - c6 ,
q
b74 = c4 - c1, b75 = c5 - c2 , b76 = c6 - c3 , b84 = c4 - c7 , b85 = c5 - c8 ,

b86 = c6 - c9, b87 = c7 - c4 , b88 = c8 - c5 , and b89 = c9 - c6 . Nonzero entries ci j in the ij
T
(
positions of Fq (q)λˆ ) q
are c11 = l7 , c14 = -l7 , c22 = l7 , c25 = -l7 , c33 = l7 ,
614
c36 = -l7, c41 = -l7 , c44 = l7 + l8, c47 = -l8 , c52 = -l7 , c55 = l7 + l8 , c58 = -l8 ,
c63 = -l7, c66 = l7 + l8 , c69 = -l8, c74 = -l8 , c77 = l8, c85 = -l8, c88 = l8 , c96 = -l8 , and
c99 = l8 . Nonzero entries dij in the ij positions of QqA (q, q& , λ) are
d11 = -k4 - k1, d1,10 = k4 , d55 = -k2 , d99 = -k3 , d10,1 = k4 , and d10,10 = -k4 . Nonzero entries ei j
in the ij positions of Qq& (q, q& , λ) are e11 = - ( l1csign(l1 ) + l 2 csign(l 2 ) ) S¢ (q& 1 ) ,
A
e55 = - ( l 3csign(l 3 ) + l 4 csign(l 4 ) ) S¢ (q& 5 ) , e99 = - ( l 5 csign(l 5 ) + l 6 csign(l 6 ) ) S¢ (q& 9 ) , and

e10,10 = -m4gS¢(q&10 ) . Finally, nonzero entries fi j in the ij positions of QAλ (q, q& , λ) are
f11 = - ( csign(l1 ) + l1dcsign(l1)) S(q& 1 ) , f12 = - ( csign(l2 ) + l2dcsign(l2 )) S(q&1 ) ,
f53 = - ( csign(l3 ) + l3dcsign(l3 )) S(q& 5 ) , f54 = - ( csign(l4 ) + l4dcsign(l4 )) S(q& 5 ) ,
f95 = - ( csign(l5 ) + l5dcsign(l5 )) S(q& 9 ) , and f96 = - ( csign(l6 ) + l6dcsign(l6 )) S(q& 9 ) .
Problem data are l1 = 5 m, l2 = 7 m , m1 = m2 = 2 kg, m3 = m4 = 6 kg , g = 9.8 m/sec ,

2
k1 = k2 = k3 = 2 N/m , k4 = 10 N/m , md = 0.13, ms = 0.2 , and vt = 20h . Initial conditions are

selected as q0 = [ 4 0 0 0 3 0 0 0 6.32 5] and
T
q& 0 = [ 0 0 0 0 0 0 0 0 0 -1] . Integration tolerances in MATLAB Code 8.3.2 of

T
Appendix 8.A that is used for simulation are utol = B tol = e-8, intol = e-6 , and Maxv = 0.7 .
With a step size h = e-4 , simulation is carried out over a 4 sec period. Results are plotted in Fig.
8.3.2. Stiction is initiated simultaneously upon onset of zero values of slip velocities of bodies
one, two, and three at approximately 1.3 sec and continues over a finite period of time. Fifteen
reparameterizations were required in 4e4 time steps (2,666 time steps per reparameterization).
To assess accuracy of simulation results, the maximum position, velocity, and acceleration
constraint error norms over the simulated time interval are 7e-9, 2e-9, and e-8, respectively.
Essentially identical results are obtained with explicit Nystrom4 and implicit trapezoidal
integration algorithms of Sections 8.1.2.4 and 8.1.2.5. This confirms that the system is not stiff.
With a slight reduction of the dynamic coefficient of friction to m d = 0.1 , position plots
very similar to the top of Fig. 8.3.2 are obtained, but the characteristic delay in onset of the
stiction event shown in Fig. 8.3.3 is obtained.
615
Figure 8.3.2 Positions and Velocities vs. Time, Translational Guide, md = 0.13, ms = 0.18
Figure 8.3.3 Velocities vs. Time, Translational Guide, md = 0.10, ms = 0.18
616
To simulate friction with the cylindrical guide shown at the lower left of Fig. 8.3.1, the
terms ( l i csign( l i ) + l jcsign(l j ) ) in generalized applied force of Eq. (8.3.5) are replaced by
l i2 + l 2j . Further, the derivative with respect to li in the rectangular form,
( csign(l ) + l dcsign(l )) , is replaced by the corresponding derivative in the cylindrical form,

i i i
l i / l + l , in foregoing expressions above. Since, in general, ( l ik + l kj ) ³

2
i
2
j l ik 2 + l kj 2 ,
friction force with the rectangular guide is greater than that with the cylindrical guide. Results for
velocities from simulations with the cylindrical guide and the same data as above for the
rectangular guide are shown on the left of Fig. 8.3.4. Note the delay in onset of stiction, similar
to that occurring in the planar three mass model and the above results with the rectangular guide.
Increasing friction coefficients to md = 0.16 and ms = 0.2 , the response shown on the right of Fig.
8.3.4 is realized.
Figure 8.3.4 Velocities vs. Time, Cylindrical Guide
Cartesian coordinate formulations of the three mass planar model problem and a four mass
spatial model problem demonstrate the ability of the continuous friction model to account for
friction and stiction effects, even for stiction constraint redundancies of degree higher than two.
Success in simulations with the explicit Nystrom4 integrator confirms that the systems are not
stiff.
617
8.4 Modeling Planar Multibody Systems with Friction
Constraint reaction forces and associated friction forces in planar revolute and
translational joints are derived and incorporated in generalized forces for use in the Index 0 DAE
form of planar system equations of motion. Derivatives of generalized forces that are required
for explicit and implicit numerical integration are derived. The approach is an extension of that
for particles in Section 8.3, with moderate complexity in the planar multibody dynamics
formulation.
8.4.1 Constraint Reaction Forces and Friction Generalized Forces
The derivation of generalized constraint reaction forces of Section 4.10 is valid for
planar systems, with Euler parameter constraints deleted. For constraint k,
Φk (qi , q j ) = 0 (8.4.1)
there exist Lagrange multipliers λ k of the same dimension as Φ k such that generalized
constraint reaction forces are given by Eq. (4.10.7), with the Euler parameter term deleted,
Qik = -Φqki T λ k
(8.4.2)
Q kj = -Φqk j T λ k
Constraint reaction force and constraint reaction torque on body i are shown in Fig.
8.4.1, represented in the body fixed x¢i - y¢i frame at joint definition point P k . The virtual work
of constraint reaction force and torque on body i is
dWik = dri¢Pk T Fi¢k + dfi Tik (8.4.3)
From Eq. (8.4.2), virtual work can also be written as
(
dWik = dqiT Qik = -dqiT Φqki T λ k = - λ kT Φqk i dqi = - λ kT Φrki dri + Φ fki dfi ) (8.4.4)
The variation of the vector equation riPk = ri + A i s¢i Pk yields driPk = dri + dfi PA is¢i Pk , which may be
transformed to dri = driPk - dfi PA is¢i Pk = A i dri¢Pk - dfi PAi s¢i Pk . Substituting this result into Eq.
(8.4.4) yields
(
dWik = - λ k T Φrki A i dri¢Pk - dfi Φ kri PA is¢i Pk + dfi Φ kfi )
(8.4.5)
( )
= -dri¢Pk T AiT Φ rki T λ k - dfi s¢i Pk T A iT PΦ rki T + i Φ kfi T λ k
Figure 8.4.1 Constraint Reaction Force and Torque on Planar Body i
618
Equating expressions for virtual work in Eqs. (8.4.3) and (8.4.5),
( )
dri¢Pk T Fi¢k + dfi Tik = -dri¢Pk T A iTΦrki T λ k - dfi s¢i Pk T A Ti PΦ rki T + i Φ fki T λ k (8.4.6)
Since Eq. (8.4.6) must hold for arbitrary δri¢Pk and dfi ,
Fi¢k = - A Ti Φrki T λ k
(8.4.7)
(
Tik = - s¢i Pk T A Ti PΦ rki T + Φ kfi T λ k)
8.4.1.1 Revolute Joint Reaction and Friction Forces
For the revolute joint of Fig. 3.2.2, constraint Jacobians of Eq. (3.2.9) are
Φrrevk
i
= -I
(8.4.8)
Φrevk
fi = -PAi s¢i Pk
So, Eq. (8.4.7) yields

Fi¢k = A iT λ k
(8.4.9)
Tik = s¢i Pk T A iT Pλ k - s¢i Pk T AiT Pλ k = 0
In the global frame,
Fik = λ k (8.4.10)
which acts at revolute joint definition point Pik on body i, as shown in Fig. 8.4.1.
The angular velocity of body j relative to body i about revolute joint definition point Pik ,
shown in Fig. 8.4.2, is w = f& - f& . With joint radius R , the velocity of the contact point on
ij j i k
body j relative to body i, shown in Fig. 8.4.2, is v ij = R k (f& j - f& i ) . The friction force on body j at
the contact point is given by Eq. (8.1.9) as
Ffjk = Fik S(v ij ,μ ) (8.4.11)
and the friction force on body i is Ffik = -Ffjk .
Figure 8.4.2 Constraint and Friction Forces and Relative Velocity in Revolute Joint
From Eq. (8.4.10), Fik = λ k and Eq. (8.4.11) is
619
Ffjk = - λ k S(R k ( f& j - f& i ), μ ) (8.4.12)
The force due to friction on body i is Ffki = - Ffjk = λ k S(R k (f& j - f& i ), μ ) , so the counterclockwise
torque acting on body i is
Tfik = R k λ k S(R k (f& j - f& i ), m) (8.4.13)
and the reaction torque on body j is Tfjk = - R k λ k S(R k (f& j - f& i ), μ ) . Generalized forces due to
friction in the revolute joint are thus
é 0 ù
Q iArevf k = ê & & ú
êë R k λ S(R k (f j - fi ), μ ) úû
k
(8.4.14)
é 0 ù
Q Arevf k
= ê & & ú
êë - R k λ S(R k (f j - fi ), μ ) úû
j k
8.4.1.2 Translational Joint Reaction and Friction Forces

For the translational joint of Fig. 3.2.3, constraint Jacobians of Eq. (3.2.15) are
é v¢i T PAiT ù
Φrtrank = ê ú
ë 0 û
i
(8.4.15)
é - v¢i T s¢i Pk - v¢i T A Ti dij ù é - v¢i T A iT ( rj + A js¢i Pk - ri ) ù
Φftrank =ê ú=ê ú
ëê - v¢i A i A j v¢j ûú êë - v¢i T A iT A j v¢j
T T
úû
i
The constraint force and constraint torque of Eq. (8.4.7) are thus
Fi¢k = - A iT [ - A i Pv¢i 0] λ k = λ1k Pv¢i
Tik = -s¢i Pk T A iT P [ - A i Pv¢i 0] λ k + éë( rjT + s¢jPk T A Tj - riT ) A i v¢i v¢i T AiT A j v¢j ùû λ k (8.4.16)
= -s¢i Pk T v¢i λ1k + éë( rjT + s¢jPk T A Tj - riT ) A i v¢i v¢i T A Ti A j v¢j ù λ k
û
where Eqs. (2.3.32) and (2.3.33) have been used. Clearly, Fi¢k is perpendicular to the axis of
relative translation, acting at translational joint definition point Pik .
For the geometry of the translational joint, with a key in body i shown in Fig. 8.4.3 that
slides in a keyway in body j, where v¢i = d i and using Eq. (8.4.16) , static equivalence of force
and torque systems requires that
f1k + f 2k + Fi¢k T ( (1/ d i )Pv¢i ) = f1k + f 2k + ( l1k / d i ) v¢i T P T Pv¢i
= f1k + f 2k + d i l1k = 0 (8.4.17)
di f 2k + Tik = 0
Thus,
620
f1k = (1/ d i ) Tik - d i λ1k
(8.4.18)
f 2k = - (1/ d i ) Tik
Figure 8.4.3 Constraint Forces and Relative Velocity in Translational Joint

The velocity of points on body j in the translational joint axis, relative to body i, is
(
vij = (1 / d i ) v iT d& ij = (1 / d i ) v¢i T A iT r& j + f& j PA js¢jPk - r&i - f& i PA is¢i Pk )
(8.4.19)
( )
= (1 / d i ) v¢i T A iT r& j + f& jPA js¢jPk - r&i - f& i (1 / d i ) v¢i T Ps¢i Pk
and the velocity of points on body i relative to body j is -vij . The friction force that acts on body
i, along the axis of relative translation, is thus
( )
Ffki = - f1k + f 2k S(v ij ,μ ) » - ( f1k csign(f1k ) + f 2k csign(f 2k ) ) S(v ij ,μ ) (8.4.20)
The virtual displacement of points on body j relative to body i on the axis of translation is
ds ij = (1/ di ) v¢i T dd ij = (1 / d i ) v¢i T A iT ( drj + df jPA js¢jPk - dri - dfi PAi s¢i Pk )
(8.4.21)
= (1/ di ) v¢i T AiT ( drj + df jPA js¢jPk - dri ) - (1 / d i ) v¢i T Ps¢i Pk
and the virtual displacement of points on body i relative to body j is -dsij . The virtual work done
by the friction force on body i is thus
dW k = - Ffki ds ij
{( dr }
(8.4.22)
= - (1/ d i ) Ffki T
j - driT ) Ai v¢i + ( df j v¢i T AiT PA js¢jPk - dfi v¢i T Ps¢i Pk )
where Eq. (2.3.33) has been used. Generalized translational joint friction forces are thus
é A v¢ ù
QiAtranfk = (1 / di ) Ffki ê T i i Pk ú
ë v¢i Ps¢i û
(8.4.23)
é - A i v¢i ù
Q Atranf k
= (1 / d ) F k
fi ê Pk ú
ë - v¢i A i PA js¢j û
j i T T
8.4.2 Derivatives of Friction Generalized Forces

For the revolute joint, differentiating Eq. (8.4.14) with respect to qij and λ k ,
Q Arevf
iq ij
k
= Q Arevf
jq ij
k
= 0 and
621
é 0 ù
Q Arevf = - Q Arevf k
= ê kT ú
( )
k
& & (8.4.24)
êë R k / λ S(R k (f j - fi ),m )λ úû
k k
iλ jλ k
where λ k = ( λ kT λ k )1/ 2 and λ k

λk
( )
= 1/ λ k λ kT have been used. Derivatives with respect to q& ij
are
é0 0 0 0 ù
QiArevf k
= -Q Arevf k
= R 2k λ k ê & - f& ),m) 0 S¢(R (f& - f& ),m) ú (8.4.25)
q& ij jq& ij
ë 0 -S¢ (R k ( f j i k j i û
For the translational joint, the absolute values f1k and f1k of f1k and f1k defined in Eq.
(8.4.18) are approximated using Eq. (8.1.13) as
f lk = f lk sign(f lk ) » f lk csign(f lk ) (8.4.26)
l = 1, 2. Derivatives of these expressions, using Eqs. (8.1.14) and (8.4.18), are
f lk = ( csign(f lk ) + f lk dcsign(f lk ) ) flkqij

q ij
(8.4.27)
f lk λk
= ( csign(f lk ) + f lk dcsign(f lk ) ) f lkλ k
and fl q& = 0 , l = 1, 2. From Eq. (8.4.18),

ij
f1kqij = -f 2kqij = (1/ d i )Tikqij

f1kλ k = (1/ d i )Tikλ k - d i [1 0] (8.4.28)
f 2kλ k = -(1/ d i )Tikλ k
Finally, from the second of Eqs. (8.4.16),

Tikqij = éë - v¢i T AiT a1 v¢i T A iT a 2 ùû
(8.4.29)
Tikλ k = -s¢i Pk T v¢i [1 0] + éë( rjT + s¢jPk T A Tj - riT ) A i v¢i v¢i T A iT A j v¢j ùû
where a 1 = éë(rjT + s¢jPk T A Tj - riT ) PA i v ¢i -v ¢i T A iT PA j v¢j ùû λ k , a 2 = éë -s¢jPk T A Tj PA i v ¢i v ¢i T A iT PA j v¢j ùû λ k ,

Eqs. (2.3.31) have been used, and from Eq. (8.4.19),
vijqij = [ 0 a 3 0 a4 ]
vijq& ij = (1/ d i ) éë - v Ti A Ti - v¢i T Ps¢i Pk v iT A iT v iT A iT PA js¢jPk ùû (8.4.30)
vijλ k = 0
( )
where a 3 = - (1 / d i ) v iT A iT P r& j + f& jPA js¢jPk - r&i and a 4 = -f& j (1 / d i ) v iT A iT A js¢jPk .
Derivatives of translational joint generalized friction forces of Eq. (8.4.23) are
622
QiAtranf
q ij
k
é A v¢ ù
ë v¢i Ps¢i û
(
= (1 / d i ) S(vij ,m) ê T i i Pk ú f1k q + f 2k q
ij ij
)
é A v¢ ù
( )
+ (1 / d i ) f1k + f 2k ê T i i Pk ú S¢(vij ,m)vijqij
ë v¢i Ps¢i û
é0 PA i v¢i 0 0 ù
( )
+ (1 / d i ) f1k + f 2k S(vij ,m) ê
ë0 0 0 0 úû
Q Atranf
jqij
k
é
= (1 / d i ) S(vij ,m) ê T T
- A i v¢i
(
ù k
Pk ú f1 q + f 2 q
ij
k
ij
)
é - A i v¢i ù
( )
+ (1 / d i ) f1k + f 2k ê T T Pk ú S¢(v ij ,m )v ijqij
é0 - PA i v¢i 0 0 ù
( )
+ (1 / d i ) f1k + f 2k S(vij ,m) ê Pk ú
ë0 - v¢i Ai A js¢j 0 v¢i A i A js¢j û
T T Pk T T
é A v¢ ù
QiAtranf
q& ij
k
( )
= (1 / d i ) f1k + f 2k ê T i i Pk ú S¢(vij ,m)vijq& ij
ë v¢i Ps¢i û
k é
- A i v¢i ù
Q Atranf
jq& ij
k
= (1 / d i ) (
f1
k
+ f 2 )
ê - v¢T A T PA s¢Pk ú S¢ (vij ,m)vijq& ij
ë i i j j û
é A i v¢i ù k
QiAtranf
λ k
k
= (1 / d i ) S(v ij , m )
ë i
(
ê v¢T Ps¢Pk ú f1 λ k + f 2 λ k
i û
k
) (8.4.31)
é - A i v¢i ù k
Q Atranf
jλ k
k
= (1 / d i ) S(v ij , m ) ê ¢ T
¢
ë - v i A i PA js j û
T (
Pk ú
f1 k + f 2k
λ λk
)
which may be evaluated using Eqs. (8.4.27), (8.4.28), (8.4.29), and (8.4.30).
Constraint reaction forces that act on planar bodies that are connected by kinematic constraints
depend on the geometry of the constraints and Lagrange multipliers that appear in the Index 0
equations of motion. Expressions for constraint contact forces are used with continuous friction
force models presented in Section 8.1 to obtain expressions for generalized friction forces that
are incorporated in Index 0 DAE of motion. Finally, derivative expressions that are required to
implement explicit and implicit numerical integration methods for multibody simulation of
planar systems with friction are presented.
623
8.5 Modeling Spatial Multibody Systems with Friction
To extend results presented for model problems with particles and planar systems, the
library of spatial joints presented in Section 3.3 is used to determine constraint reaction forces
and associated friction forces for spatial multibody dynamic simulation. Derivatives that are
required in the Index 0 DAE formulation and for numerical integration of the equations of
motion are presented. The similar geometry of cylindrical, revolute, and translational joints
defined in Section 3.3 is exploited for formulation and coding efficiencies, but requires care in
evaluating expressions that arise in each of the three constraints.
8.5.1 Constraint Contact Reaction Forces
Constraint joint reaction force and torque on body i due to joint k shown in Fig. 8.5.1,
represented in the x ¢i - y¢i - z¢i frame, are given by Eq. (4.10.17) as
é Fi¢ k ù é - A Ti Φ rki T λ k ù
ê kú = ê ú (8.5.1)
ë Ti¢ û ( )
ê - (1 / 2)G ( p i )Φ pk i T - s% ¢i k A Ti Φ rki T λ k ú
ë û
It is important to note that, whereas Lagrange multipliers associated with Euler parameter
normalization conditions appear in the Index 0 DAE of Eq. (5.5.12), they do not appear in the
constraint reaction force and torque of Eq. (8.5.1). While Eq. (8.5.1) holds for all joints, it is
valid only if constraint Jacobian terms Φ rk and Φ pk and associated Lagrange multipliers λ k are
i i
properly defined for each joint.
Figure 8.5.1 Constraint Reaction Force and Torque on Spatial Body i

Constraint surface contacts and associated physical forces of constraint depend on the
geometry of joints. For the cylindrical joint of Section 3.3.3, shown in Fig. 8.5.2, rotational and
translational bearings are located in body i at endpoints Pik and Q ik that are in contact with the
cylindrical bar in body j. Constraint contact force components in cylindrical joint k that act at
ends Pik and Q ik in body i are fx¢ , fy¢ , fx¢ , and fy¢ , as shown in Fig. 8.5.2. Contact force fz¢ is
1k 1k 2k 2k 1k
the contribution from a revolute joint thrust bearing and does not act in the cylindrical joint.
624
Figure 8.5.2 Constraint Forces for Cylindrical and Revolute Joints
Cylindrical joint constraint reaction force F¢ and torque T¢ shown in Fig. 8.5.2, that act
k k
i i
on body i at point Pi and are represented in the body x ¢i - y¢i - z¢i reference frame, are determined
k
by Eq. (8.5.1), using submatrices of the Jacobian of the cylindrical constraint of Eq. (3.3.24).
These subJacobians are obtained for the cylindrical joint, using Jacobians of building block
constraints in Section 3.3.2. Software implementation is included in Code 5.8 of Appendix 5.A.
Force F¢ and torque T¢ must be converted to statically equivalent physical contact force
k k
i i
components fx¢ , fy¢ , fx¢ , and fy¢ shown in Fig. 8.5.2. Summing forces on body i in the
1k 1k 2k 2k
v¢xikand v¢yik directions,
v¢xik T Fi¢ k + f x¢1k + f x¢2 k = 0

(8.5.2)
v¢yik T Fi¢k + f y¢1k + f y¢2 k = 0
Summing torques about v¢x and v¢y ,

ik ik
v¢xik T Ti¢k - a k f y¢2k = 0

(8.5.3)
v¢yik T Ti¢k + a k f x¢2k = 0
The solution of Eqs. (8.5.2) and (8.5.3) is contact forces that are statically equivalent to F¢ and T¢
k k
i i
,
f x¢1k = - v ¢xik T Fi¢k + (1 / a k ) v ¢yik T Ti¢k
f y¢1k = - v ¢yik T Fi¢k - (1 / a k ) v ¢xik T Ti¢k
(8.5.4)
f x¢2 k = - (1 / a k ) v ¢yik T Ti¢k
f y¢2 k = (1 / a k ) v ¢xik T Ti¢k
The magnitudes of resultant contact forces on body i at points Pik and Q ik are
625
f ¢l k = (f x¢ lk ) 2 + (f y¢l k ) 2 , l = 1, 2 (8.5.5)
For the revolute joint, Fi¢ and Ti¢ are calculated using Eq. (8.5.1), with submatrices of the
k k
Jacobian of the revolute constraint of Eq. (3.3.26) defined in Section 3.3 and evaluated using
software in Code 5.8. In addition to the lateral forces of Eqs. (8.5.4) and (8.5.5) that are defined
using the revolute constraint Jacobian, the contact force fz¢ shown in Fig. 8.5.2 is the thrust due
1k
to the constraint of Eq. (3.3.25),

fz¢1k = v¢zikTFi¢k (8.5.6)
For the translational joint, Fi¢ and Ti¢ are calculated using Eq. (8.5.1), with submatrices
k k
of the Jacobian of the translational constraint of Eq. (3.3.28) defined in Section 3.3 and evaluated
using software in Code 5.8. A key of length b along the negative v¢yik axis attached at point P k
rides in a slot in body j that is parallel to v ¢zik , to enforce the constraint that precludes relative
rotation of bodies i and j about the axis defined by v¢zik , as shown in Fig. 8.5.3.
The effect of the rotational constraint of Eq. (3.3.27) and associated constraint
torque about vector v¢z determines the reaction force fx¢ shown in Fig. 8.5.3. Assuming that a
ik 3k
bearing surface on the v¢y axis that precludes relative displacement in the v¢x direction, hence
ik ik
k
precluding relative rotation about v¢zik , is at a distance b k from point Pi , the moment condition
about v¢z is v¢z Ti¢ + b fx¢ = 0 . Thus,
ik ikT k k 3k
fx¢3k = - (1/ bk )v¢zikTTi¢k (8.5.7)
Figure 8.5.3 Constraint Forces for Translational Joint
626
Summing forces on body i in the v¢xik and v¢yik directions,
v¢xik T Fi¢k + f x¢1k + f x¢3k + f x¢2k = 0

(8.5.8)
v¢yik T Fi¢k + f y¢1k + f y¢2 k = 0
and summing torques about v¢xik and v¢yik ,
v¢xik T Ti¢k - a k f y¢2k = 0

(8.5.9)
v¢yik T Ti¢k + a k f x¢2k = 0
The solution of Eqs. (8.5.8) and (8.5.9) is contact forces that are statically equivalent to
Fi¢ and Ti¢ ,
k k
f x¢1k = - v¢xik T Fi¢k + ( (1 / a k ) v¢yik T + (1 / b k ) v¢zik T ) Ti¢k

f y¢1k = - v¢yik T Fi¢k - (1 / a k ) v¢xik T Ti¢k
(8.5.10)
f x¢2 k = - (1 / a k ) v¢yik T Ti¢k
f y¢2 k = (1 / a k ) v ¢xik T Ti¢k
Clearly, joint contact forces depend on the geometry of the translational joint. A two
contact point geometry that is added to the cylindrical constraint is presented in (Haug, 2018c),
yielding different results, as would more intricate models of contacts that occur in alternative
translational joint designs.
8.5.2 Friction Forces in Joints
In order to avoid difficulties with the discontinuous sign function of slip velocity in the
Coulomb friction model and to account for friction effects when sliding velocity is near zero, the
continuous friction model of Section 8.1.2.2 is used. Since spherical joints are generally
lubricated and subject to low friction effects, friction is considered here for only cylindrical,
revolute, and translational joints.
For the cylindrical joint, constraint reaction forces defined in Section 8.5.1.1 are first
computed, using the cylindrical constraint Jacobian. Since the vector dij in Fig. 3.3.4 is in the
k
opposite direction of vz , the velocity of contact points in body i relative to body j in the direction
ik
vikz is
s& ijk = - v zik T d& ijk = - v ¢zik ¢ T A iT ( r& j + B (p j , s ¢jk )p& j - r&i - B ( p i , s ¢i k )p& i ) (8.5.11)
The friction force that acts on body i in the direction vz of translation is

ik
f ijkcylfr = - FijkcylS ( s& ijk ,μ t ) (8.5.12)
where m t contains translational coefficients of friction and the sum of normal contact forces at
contact points Pik and Q ik , from Eq. (8.5.5), is
627
Fijkcyl º f ¢1k + f ¢2k (8.5.13)
Using the identity w = 2 E ( p ) p& of Eq. (2.8.49), the angular velocity of body i relative to
body j, about the axis defined by vz , is
ik
z ( ωi - ω j ) = 2 v z
wijk = v ikT ¢ik T A iT ( E(p i )p& i - E(p j )p& j ) (8.5.14)
The torque due to friction on body i relative to body j about vz , where Rk is the radius of the
ik
rotational bearing in the joint, and rotation induced slip velocity is R wij , is
k k
tijkcylfr = -Rk FijkcylS(Rkwijk , μr ) (8.5.15)
where μr contains rotational coefficients of friction.

Replacing w with dπ in Eq. (2.8.68) and derivative with differential in Eqs. (8.5.11) and
(8.5.14), virtual displacement and rotation of body i relative to body j, along and about vz , are
ik
ds ijk = - v ikz T dd kij = - v ¢zik T A iT ( drj + B (p j , s¢jk )dp j - dri - B (p i , s¢i k )dp i )
(8.5.16)
dpijk = v ikz T ( dπ i - dπ j ) = 2 v ¢zik T A iT ( E (p i )dp i - E (p j )dp j )
The virtual work of friction forces that act in cylindrical joint k is thus
T T
dW k = QikcylfrT éëdriT dp Ti ùû + Q kcylfr
j
T
éë drjT dp Tj ùû = dsijk fijkcylfr + dpijk τijkcylfr
= - v¢zikT AiT ( drj + B(p j , s¢jk )dp j - dri - B(pi , s¢i k )dpi ) f ijkcylfr (8.5.17)
+ 2 v¢zikT AiT ( E(pi )dpi - E(p j )dp j ) tijkcylfr
so generalized forces acting on bodies i and j due to friction in the cylindrical joint are
é A i v ¢zikf ijkcylfr ù
Q kcylfr
=ê ú
ë B ( p i , s¢i ) A i v ¢z f ij + 2 E( p i ) T A i v ¢zikt ijkcylfr û
i k T ik kcylfr
(8.5.18)
é - A i v ¢zikf ijkcylfr ù
Q kcylfr =ê ú
ë - B (p j , s ¢j ) A i v ¢z f ij - 2 E ( p j ) T A i v ¢zikt ijkcylfr û
j k T ik kcylfr
For the revolute joint, constraint reaction forces defined in Section 8.5.1.2 are first
computed, using the revolute constraint Jacobian. The friction torque for the revolute joint is the
torque of Eq. (8.5.15) for the cylindrical joint, since s&ij = dsij = 0 as a result of the thrust
k k
constraint, plus the effect of thrust reaction force fz¢ = v¢z Fi¢ of Eq. (8.5.6). Assuming that the
1k ik T k
thrust bearing has radius Rk , the slip velocity is R wij and the friction torque acting on body i
k k
about the vz axis due to the thrust bearing is

ik
628
tijkrev5fr = - R k f z¢1k S(R k wijk , μ r )
(8.5.19)
» - R k f z¢1k csign(f z¢1k )S(R k wijk , μ r )
The virtual work due to this torque and that of Eq. (8.5.15), computed using revolute constraint
subJacobians, is
dW k = dpijk ( tijkrev5fr + tijkcylfr ) = 2 v¢zik T A iT ( E(p i )dp i - E(p j ) dp j ) ( tijkrev5fr + tijkcylfr )
so generalized forces that act on bodies i and j due to friction in the revolute joint are
é 0 ù
Q krevfr = ê kcylfr ú
êë 2E (pi ) A i v¢z ( tij + τ ij ) úû
i T ik krev5fr
(8.5.20)
é 0 ù
Q krevfr = ê ú
êë -2E (p j ) A i v¢z ( tij + τijkcylfr ) úû
j T ik krev5fr
For the translational joint, constraint reaction forces defined in Section 8.5.1.3 are first
computed, using the translational constraint Jacobian. The friction force for the translational joint
has only a translational component, since wij = dpij = 0 , due to the rotational constraint. Since the
k k
translational joint contact geometry tends to be rectangular, in contrast to the rotational bearings
in cylindrical and revolute joints, each of the contact force components of Eqs. (8.5.10) and
(8.5.7) acts independently. This yields the friction translational force in the vz direction,
ik
fijktranfr = -FijktranS(s&ijk ,μt ) (8.5.21)
where
Fijktran = f x¢1k + f y¢1k + f x¢2k + f y¢2k + f x¢3k
» f x¢1k csign(f x¢1k ) + f y¢1k csign(f y¢1k ) + f x¢2k csign(f x¢2k ) (8.5.22)
+ f y¢2k csign(f y¢2k ) + f x¢3k csign(f x¢3k )
The virtual work of this force is

dW ktranfr = ds ijk f ijktranfr = - v¢zikT A iT ( drj + B (p j , s¢jk )dp j - dri - B (p i , s¢i k )dp i ) f ijktranfr
so generalized forces acting on bodies i and j due to friction in the translational joint are
é Ai v¢zik ù ktranfr
Q iktranfr = ê T ik ú ij
f
ëB (p i , s¢i ) A i v¢z û
k
(8.5.23)
é - A i v¢zik ù ktranfr
Qj ktranfr
=ê T ik ú ij
f
ëê -B (p j , s¢j ) A i v¢z ûú
k
629
8.5.3 Derivatives of Friction Forces
For each of the three joints analyzed, using the associated constraint Jacobian, Eq. (8.5.1)
is differentiated with respect to qi j = éëqi qj ùû and λ k to obtain
T T T
(
Fi¢qki j = - A iT Φ kri T λˆ k )qi j
- éë 0 C(p i , Φ rki T λ k ) 0 0 ùû
æ
Ti¢qki j = - ç (1 / 2)G (p i ) Φ pk i T λˆ k
è
( ) qi j
(
- s% ¢i k A iT Φrki T λˆ k ) ö
÷
qi j ø
- éë 0 -(1 / 2)G (Φ kpi T λ k ) - s% ¢i k C(p i , Φrki T λ k ) 0 0 ùû (8.5.24)

Fi¢λkk = - A iT Φrki T
(
Ti¢λkk = - (1 / 2)G (p i )Φ pki T - s% i¢k A Ti Φ rki T )
where Eqs. (2.8.30) and (2.8.44) and identities of Section 2.6 have been used. The terms
( )
ΦTri λˆ (
and ΦTpi λˆ
qi j
)
in Eqs. (8.5.24) are rows of
qi j
(Φ k T
qi λˆ k ) qij
é
ë
(
= ê Φqk i T λˆ k ) (Φ
qi
k T
qi λˆ k) ùúû qj
(
é Φ k T λˆ k
ê ri
) (Φ
ri
kT
ri λˆ )k
(Φ
pi
kT
ri ) (Φ
λˆ k
rj
kT
ri )
λˆ k
pj
ù
ú
(8.5.25)
=ê ú
ë
(
ê Φpi λˆ
k T k
) (Φ
ri
k T
pi λˆ )
k
(Φ
pi
k T
pi λˆ ) ( Φ
k
rj
k T
pi λˆ )
k
ú
pj û
which are defined in Eq. (5.3.34) as the matrix P4(qi j , l k ) , for each of the spatial joints in
Appendix 5.B, and evaluated using software in Code 5.8.
For the cylindrical joint, using the associated constraint Jacobian and matrix P4(qi j , l k ) ,
derivatives of terms in Eqs. (8.5.4) with respect to qi j are obtained using the first two of Eqs.
(8.5.24),
f x¢1k ¢ik T Fi¢qki j + (1/ a k ) v¢yik T Ti¢qki j
qi j = - v x
f y¢1k ¢ik T Fi¢qki j - (1 / a k ) v¢xik T Ti¢qki j

qi j = - v y
(8.5.26)
f x¢q2ki j = -(1/ a k ) v¢yik T Ti¢qki j
f y¢q2ki j = (1 / a k ) v¢xik T Ti¢qki j
Expressions for derivatives with respect to λ k are obtained by replacing subscript qi j with λ k in
Eqs. (8.5.26), using the third and fourth of Eqs. (8.5.24).
Derivatives of Eqs. (8.5.13) with respect to qi j and λ k are
630
¢1k ¢2k
qi j = f qi j + f qi j
Fijkcyl
(
= (1/ f ¢1k ) f x¢1k f x¢q1ki j + f y¢1k f y¢1k )
qi j + (1/ f (
¢2k ) f x¢2k f x¢q2ki j + f y¢2k f y¢q2ki j ) (8.5.27)
Fijkcyl
λk
= (1/ f ¢1k ) (f ¢x f ¢ + f y¢1k f y¢λ1kk
1k 1k
xλ k ) + (1/ f ¢ ) ( f ¢
2k
x
2k
f x¢λ2kk + f y¢2k f y¢λ2kk )
Derivatives of Eqs. (8.5.11), and (8.5.14) with respect to qi j are
ì é0 C(pi , (r&j + B(p j , s¢jk )p& j - r&i - B(pi , s¢i k )p& i )) 0 0ù ü

ïë ûï
s& = - v¢ í
k
ijqi j z
ik T
ý
ïî+ Ai éë0 -B(p& i , s¢i ) 0 B(p& j , s¢j ) ùû
T k k
ïþ (8.5.28)
wijkqi j = 2 v¢zikT éë0 C(pi , (E(pi )p& i - E(p j )p& j )) - AiT E(p& i ) 0 AiT E(p& j ) ùû
where Eqs (2.8.14) and (2.8.28) have been used. Since s&ij and wij do not depend on λ k ,
k k
s& ijkλ k = wijkλ k = 0 . Using these results in Eqs. (8.5.12) and (8.5.15), derivatives with respect to qi j
are
qi j = - Fij S ( sij ,μ t ) sijqi j - S ( s ij ,μ t ) Fijqi j

fijkcylfr kcyl
¢ &k &k &k kcyl
(8.5.29)
τ kcylfr
ijqi j = -(R k 2
) F S¢(R k wijk ,μ r )wkijqi j - R kS(R k wijk ,μ r )Fijkcyl
kcyl
ij qi j
Since s&ij and wij do not depend on λ k ,

k k
f ijkcylfr
λk
= -S ( s& ijk ,μ t ) Fijkcyl
λk
(8.5.30)
τ ijkcylfr
λk
= - R k S(R k wijk ,μ r )Fijkcyl
λk
To account for derivatives with respect to q& ij , from Eqs. (8.5.11) and (8.5.14),
s&ijkq& ij = - v¢zikT ATi éë-I -B(pi , s¢i k ) I B(p j , s¢jk ) ùû

(8.5.31)
wijkq& ij = 2v¢zikT ATi éë0 E(pi ) 0 -E(p j ) ùû
Since Fij¢kcyl does not depend on qi j , from Eqs. (8.5.12) and (8.5.15),
fijkcylfr
q& ij = -FijkcylS¢ ( s&ijk ,μ t ) s& ijkq& ij
(8.5.32)
q& ij = - ( R ) Fij S (R wij ,μ r )wijq& ij
2
τijkcylfr k kcyl
¢ k k k
The foregoing enable calculation of derivatives of generalized friction forces of Eq.

(8.5.18) with respect to qi j , λ , and q& ij , using derivative identities of Section 2.6, as
k
631
é Ai v¢zikf ijkcylfr
qi j + a1 ù
Q kcylfr
iqi j =ê ú
êëB(pi , s¢i ) Ai v¢z fijqi j + 2E(pi ) Ai v¢z tijqi j + a2 úû
k T ik kcylfr T ik kcylfr
é -Ai v¢zikfijkcylfr
qi j - a1
ù
Q jqi j = ê
kcylfr
ú
êë-B(p j , s¢j ) Ai v¢z fijqi j - 2E(p j ) Ai v¢z tijqi j - a3 úû
k T ik kcylfr T ik kcylfr
a1 = éë0 B(pi , v¢zikfijkcylfr ) 0 0ùû

a2 = éë0 K(s¢i k , Ai v¢zikfijkcylfr ) + B(pi , si¢k )T B(pi , v¢zikf ijkcylfr ) 0 0ùû (8.5.33)
+ 2 éë0 R ( Ai v¢ziktijkcylfr ) + E(pi )T B(pi , v¢ziktkcylfr
ij ) 0 0ùû
a3 = éë0 B(p j , s¢jk )T B(pi , v¢zikfijkcylfr ) 0 K(s¢jk , Ai v¢zikf ijkcylfr ) ùû
+ 2 ëé0 E(p j )T B(pi , v¢ziktijkcylfr ) 0 R ( Ai v¢ziktijkcylfr )ûù
é Ai v¢zikf ijkcylfr
λk
ù
Q kcylfr
=ê ú
êëB(pi , s¢i ) Ai v¢z f ijλ k + 2E (pi )Ai v¢z tijλ k úû
iλ k k T ik kcylfr T ik kcylfr
(8.5.34)
é -Aiu¢zikf ijkcylfr
λk
ù
Qkcylfr =ê ú
êë-B(p j , s¢j ) Ai v¢z fijλ k - 2E(p j ) Ai v¢z tijλ k úû
jλ k k T ik kcylfr T ik kcylfr
é Ai v¢zikfijkcylfr
q& ij
ù
Q kcylfr
iq& ij =ê T ú
êëB (pi , s¢i )Ai v¢z fijq& ij + 2E (pi )Ai v¢z tijq& ij úû
k ik kcylfr T ik kcylfr
(8.5.35)
é -Ai v¢zikfijkcylfr
q& ij
ù
Qkcylfr
jq& ij =ê T ú
êë -B (p j , s¢j )Ai v¢z fijq& ij - 2E (p j ) Ai v¢z tijq& ij úû
k ik kcylfr T ik kcylfr
For the revolute joint, using the associated constraint Jacobian and matrix P4(qi j , l k ) ,
derivatives of Eq. (8.5.26) are evaluated. Expressions for derivatives with respect to λ k are
likewise obtained by replacing subscript qi j with λ k in Eqs. (8.5.26). The derivative of Eq.
(8.5.6) is
f z¢q1ki j = v ¢zik T Fi¢qki j (8.5.36)
which is evaluated using the first of Eqs. (8.5.24). The associated derivative with respect to λ k is
evaluated using the third of Eqs. (8.5.24). Similarly, the derivatives of Eqs. (8.5.27) are
evaluated. Since relative translation is zero, only the second of Eqs. (8.5.30) and (8.5.31) are
germane. Finally, the derivative of the torque of Eq. (8.5.19) with respect to qi j is
= - ( R k ) f z¢1k csign(f z¢1k )S¢(R k wijk ,μ r )wijkqi j

2
tijkrev5fr
qi j
(8.5.37)
- R k S(R k wijk ,μ r ) ( csign(f z¢1k ) + f z¢1k dcsign(f z¢1k ) ) f z¢q1ki j
632
Since wij does not depend on λ k , the derivative of this quantity with respect to λ k is
k
tijkrev5fr
λk
= -R kS(R k wijk ,μ r ) ( csign(f z¢1k ) + fz¢1k dcsign(f z¢1k ) ) f z¢λ1kk (8.5.38)
To account for derivatives of Eq. (8.5.19) with respect to q& ij , from Eq. (8.5.31),
= - ( R k ) f z¢1k csign(f z¢1k )S¢(R k wijk ,μ r )wijkq& ij

2
tijkrev5fr
q& ij (8.5.39)
The foregoing enables calculation of derivatives of revolute generalized friction forces of

Eqs. (8.5.20) with respect to qi j , λ , and q& ij as
k
é 0 ù
= ê ú
( )
Q krevfr
êë 2E(pi ) A i v ¢z tijqi j + τijqi j + b1 úû
iq i j T ik krev5fr kcylfr
é 0 ù
= ê ú
( )
Q krevfr (8.5.40)
êë -2E(p j ) A i v ¢z tijqi j + τijqi j - b 2 úû
jq i j T ik krev5fr kcylfr
b1 = 2 éë 0 R ( A i v¢zik ) + E(p i )T B(p i , v¢zik ) 0 0 ùû ( tijkrev5fr + τijkcylfr )

b 2 = 2 ëé 0 E(p j )T B(p i , v ¢zik ) 0 R ( A i v ¢zik ) ûù ( tijkrev5fr + τ ijkcylfr )
é
(
ù krev5fr kcylfr
)
0
Qikrevfr
λ k = ê2E(p ) A v¢ ú tijλk + τijλ k
T ik
ë i i z û
(8.5.41)
é 0 ù krev5fr kcylfr
Qkrevfr
jλ k = ê
ë -2E(p j ) Ai v¢z û
T ik (
ú tijλ k + τijλ k )
é 0 ù
=ê ú
( )
Q krevfr
êë2E(pi ) Ai v¢z tijq& ij + τijq& ij úû

iq& ij T ik krev5fr kcylfr
(8.5.42)
é 0 ù
=ê ú
( )
Qkrevfr
êë -2E(p j ) Ai v¢z tijq& ij + τijq& ij úû
jq& ij T ik krev5fr kcylfr
For the translational joint, using the associated constraint Jacobian and matrix P4(qi j , l k ) ,
derivatives of Eq. (8.5.10) with respect to qi j are
f x¢1k ¢ik T Fi¢qki j + ( (1/ a k ) v¢yik T + (1/ b k ) v¢zik T ) Ti¢qki j

qi j = - v x
f y¢1k ¢ik T ¢k ¢ik T Ti¢qki j

q i j = - v y Fiqi j - (1 / a ) v x
k
(8.5.43)
f x¢q2ki j = -(1/ a k ) v¢yik T Ti¢qki j
f y¢q2ki j = (1 / a k ) v¢xik T Ti¢qki j
633
Expressions for derivatives with respect to λ k are likewise obtained by replacing subscript qi j
with λ k in Eqs. (8.5.43). The derivative of Eq. (8.5.7) with respect to qi j is
f x¢q3ki j = (1 / b k ) v ¢zik T Ti¢qki j (8.5.44)
which is evaluated using the second of Eqs. (8.5.24). The associated derivative with respect to
λ k is evaluated using the fourth of Eqs. (8.5.24).
The derivative of Eq. (8.5.22) with respect to qi j is
qi j º ( csign(f x ) + f x dcsign(f x ) ) f xqi j + ( csign(f y ) + f y dcsign(f y ) ) f yq i j

Fijktran ¢1k ¢1k ¢1k ¢1k ¢1k ¢1k ¢1k ¢1k
+ ( csign(f x¢2k ) + f x¢2k dcsign(f x¢2k ) ) f x¢q2ki j + ( csign(f y¢2k ) + f y¢2k dcsign(f y¢2k ) ) f y¢q2ki j (8.5.45)
+ ( csign(f x¢3k ) + f x¢3k dcsign(f x¢3k ) ) f x¢q3ki j
and similarly with respect to λ k . Derivatives of Eq. (8.5.21) are thus

fijktranfr
qi j = - Fijktran S¢(s& ijk ,μ t )s&ijk qi j - S(s& ijk ,μ t )Fijktran
qi j
fijktranfr
λk
= -S(s& kij ,μ t )Fijktran
λk
(8.5.46)
fijktranfr
q& ij = - Fijktran S¢ ( s& kij ,μ t ) s&ijkq& ij
The foregoing enables calculation of derivatives of translational generalized friction

forces of Eqs. (8.5.23) with respect to qi j , λ , and q& ij as
k
é A i v¢zikf ijktranfr
qi j + c1 ù
Q ktranfr
iq i j =ê ú
êë B(p i , s¢i ) A i v¢z f ijqi j + c 2 úû
k T ik ktranfr
é - A i v ¢zikf ijktranfr
qi j - c1 ù
Q jq i j = - ê
ktranfr
ú
êë - B(p j , s¢j ) A i v ¢z f ijqi j - c3 úû
k T ik ktranfr
c1 = éë0 B(p i , v¢zik ) 0 0 ùû f ijktranfr (8.5.47)

c2 = ëé0 K (s¢i k , A i v ¢zik) + B(p i , s¢i k )T B(p i , v¢zik ) 0 0 ùû f ijktranfr
c 3 = ëé0 B(p j , s¢i k )T B(pi , v¢zik ) 0 K (s¢jk , A i v ¢zik) ûù f ijktranfr
é Ai v¢zik ù ktranfr
Q ktranfr = ê ú f ijλ k
ë B(p i , s¢i ) Ai v¢z û
k
iλ k T ik
(8.5.48)
é - Ai v¢zik ù ktranfr
Q ktranfr = ê ik ú ij λ k
f
jλ k
ë - B( p j , s ¢
j
k T
) A i v ¢
z û
634
é A( p i ) v¢zik ù ktranfr
Q iktranfr
q& ij = ê ik ú ijq& ij
f
ë B(p i , s¢i ) A i v¢z û
k T
(8.5.49)
é - A( p i ) v¢zik ù ktranfr
Q ktranfr = ê ik ú ijq& ij
f
ë -B( p j , s¢j ) Ai v¢z û
jq& ij k T
Constraint reaction forces at joint definition points derived using results of Section 4.10 are
converted to equivalent contact forces in spatial joints that are defined in Section 3.3. Relative
velocities at contact points are used with contact forces and the continuous friction model of
Section 8.1.2.2 to obtain generalized friction forces that act on the multibody system.
Expressions for derivatives of friction forces that are required for implementation of Index 0
DAE integration algorithms are derived. Thankfully, these results hold in perpetuity.
635
8.6 Index 0 DAE Formulation for Systems with Friction
As shown in Sections 8.1 and 8.3, the Index 0 DAE formulation of Section 5.5 is well
suited for simulation of multibody systems with friction. Its formulation includes Lagrange
multipliers that determine constraint contact forces that are used in Sections 8.4 and 8.5 to derive
expressions for generalized friction forces for planar and spatial systems, respectively. Once
generalized friction forces are available, implementation of the Index 0 formulation for solution
of the equations of motion of planar and spatial systems is identical, as presented in this section.
8.6.1 Index 0 Equations of Motion
Including the effects of friction forces derived in Sections 8.4 and 8.5, system equations
of motion with friction are of the form
&& + ΦTq (q,t)λ = Q A (t,q, q& , λ,μ ) + S(q, q& )
Mq (8.6.1)
where the term S(q, q& ) is zero for planar systems that are modeled with centroidal generalized
q = D(q, t )&&
coordinates. Substituting the relation && v - UB(q ) γ(q, q,
& t ) of Eq. (5.2.31) into Eq. (8.6.1)
yields the Index 0 equations of motion
M(q,t)D(q,t)&&v + FTq (q,t)λ = M(q,t)UB(q,t)g(q, q& ,t) + QA (t,q,q& , λ,μ) + S(q,q& ) (8.6.2)
with constraint equations in Cartesian generalized coordinates that are satisfied by the tangent
space parameterization of Section 5.2,
F (q, t) = 0
F q q& = -F t = ν (8.6.3)
(( ) )
&& = - F q qˆ& q& + 2F qt q& + F tt º - γ
F qq
q
where arguments of functions that appear in the last two equations have been suppressed. With
the exception of dependence of Q (t, q, q& , λ, μ) on λ and μ , the Index 0 DAE of Eqs. (8.6.2) and
A
(8.6.3) are as presented in Section 5.5. This exception, however, leads to significant challenges in
numerical solution of the equations of motion with friction. First, equivalence of the DAE of Eqs.
(8.6.2) and (8.6.3) with an ODE does not follow from the analysis of Section 5.3, so justification
for direct application of an ODE integrator presented in Sections 5.5.4 and 5.5.5 does not hold.
Second, the derivative of Q (t, q, q& , l, m) with respect to λ is required for use of even explicit
A
numerical integration methods, and certainly for use of implicit numerical integration methods.
To resolve these issues, explicit and implicit numerical integration algorithms are presented in
Sections 8.6.2 and 8.6.3, respectively. Extensions of MATLAB Codes 5.7 and 5.9 to account for
friction in planar and spatial systems are presented in Sections 8.7 and 8.9, respectively.
8.6.2 Explicit Numerical Integration Algorithm
The tangent space parameterization of holonomic constraints of Eqs (8.6.3) is not
influenced by dependence of Q (t, q, q& , λ,μ) on λ. Thus, all kinematically related terms are
A
available for use in numerical computation of &&

v and λ , once v and v& are determined by initial
636
conditions or prior integration results. Equation (8.6.2), however, is nonlinear in λ and may be
written in residual form as
R(&&v, λ) º MDv
&& + FTq λ - QA - MUBg - S = 0 (8.6.4)
with v and v& , hence q an d q& , known. The Jacobian of R with respect to &&
v and λ is
R &&v ,λ = éë MD F Tq - Q Aλ ùû (8.6.5)
While the matrix éë MD F Tq ùû is shown in Section 5.5.1 to be nonsingular, inclusion of the term
- QAλ in Eq. (8.6.5) raises the possibility that R&&v,λ is singular. If so, friction effects induce
singular behavior. This possibility must be monitored during simulation.
To determine &v&n and l n for a time t n at which vn and v& n are known, Newton-Raphson
iteration for &v&n and l n is
é D&&
vi ù
R &&v,λ ê i ú = -R (&& vi , l i )
ëDl û
, i = 1L until R £ intol (8.6.6)
é&& vi+1 ù é&& vi ù é D&&
vi ù
ê i+1 ú = ê i ú + ê i ú
ël û ël û ë Dl û
where &&
1 1
v and λ are solution estimates at t n , perhaps &&v1 = &&v n -1 and λ1 = λ n -1 , where &&vi-1 and λ i-1
are the solution at time step t n -1 .
v1 and l1 at t1 = t that satisfy Eq. (8.6.2) are more difficult to obtain, since an
0
Values of &&
estimate of their values is not readily available. Equation (8.6.2) may be solved for &&v 0 and l 0
using Q (t ,q , q& , 0,0) , where q 0 and q& 0 are initial values that satisfy the first two of Eqs. (8.6.3).
A 0 0 0
v 0 and l 0 , with the nonsingular

With this value of generalized force, Eq. (8.6.2) is linear in &&
coefficient matrix éëMD FqT ùû . Since these values of &&
v 0 and l 0 may not be accurate enough to
assure convergence in Eq. (8.6.6), a parameterized system of equations may be used. Setting
μ(w) = wμ , 0 £ w £ 1 , &&
v 0 and l 0 are the solution of Eq. (8.6.2) with w = 0. Indexing w k = k/N ,
k = 1,...,N, Eq. (8.6.6) may be used with w = 1/N and &v&0 and λ0 . With this estimate, convergence
of Eq. (8.6.6) yields &&v1 and l1 . The process may be continued with w k = k/N , k = 2,...,N, to
obtain initial values &&v N = &&
v1 and λ N = λ1 that satisfy Eq. (8.6.2) at t 0 .
It is interesting that the foregoing sequence of computations to obtain initial values of
&&
v and λ is required in the friction problem, due to the fact that Eq. (8.6.2) cannot be reduced to an
ODE. This is in contrast to the situation in Section 5.5 without friction, in which only
kinematically admissible initial conditions on position and velocity are required to initiate the
numerical integration process. The distinction is that Index 0 DAE without friction are equivalent
to ODE, but those with friction are not.
637
With convergence of Eq. (8.6.6), &v&n is defined as &&
v n = ODEfunct(v n , v& n ) , and an
explicit numerical integration algorithm may be applied to advance to time step t n +1 with
vn+1 and v& n+1 . Bounds on the number of iterations required in Eq. (8.6.6) and the condition number
of R&&v,λ are monitored as criteria for reparameterization. If unbounded growth in the condition
number of R&&v,λ occurs, the possibility of singular behavior of the system must be considered.
The explicit numerical integration algorithm with friction is as follows:
(1) Define initial conditions q and q& at t 0 that satisfy kinematic configuration and
0 0
velocity constraints of Eq. (8.6.3). Evaluate the constraint Jacobian Fq (q ,t ) and matrices
0 0
U and V in Eqs. (5.2.5) and (5.2.6). Obtain initial conditions v = 0 and &v = V &q from Eqs.
0 0 T 0
(5.2.51). Evaluate B0 of Eq. (5.2.12) and update B in Eq. (5.2.20). Evaluate u = h ( v , t) of

Eq. (5.2.14) and q of Eq. (5.2.33). Evaluate D of Eq. (5.2.24) and q& of Eq. (5.2.34).
Evaluate γ of Eq. (5.2.35).
(2) Evaluate R &&v ,λ of Eq. (8.6.5) and iteratively determine &&vn and λn in Eq. (8.6.6). Apply an
explicit numerical integrator to determine vn+1 and v& n+1 . Use Eqs. (5.2.33) and (5.2.34) to
determine qn+1 and q& n+1 .
(3) Monitor the condition number of R &&v ,λ , the norm of v, and the number of iterations
required to evaluate &v&i , u, and B. If tolerances are exceeded, define a new time t 0 and
associated q and q& . Repeat calculations in Step (1) to define a new parameterization and
0 0
initial conditions v 0 and v& 0 . This process follows the trajectory shown in Fig. 5.2.2, moving
smoothly across charts on the constraint manifold.
(4) Continue the process until the final time tf is reached, or a singularity occurs.
While Lagrange multipliers are not used in application of the explicit integrator, they are
computed in Eq. (8.6.6) and may be used to determine constraint reaction forces in the system.
8.6.3 Implicit Numerical Integration Algorithm
An approach for applying implicit Runge-Kutta methods, including the trapezoidal
method, for solution of Eq. (8.6.2) has been suggested by Gear and Petzold (1984;1986). They
show that direct application of Runge-Kutta methods for solution of Index 1 DAE, which
includes Eq. (8.6.2), yields acceptable results. The approach is a formal; i.e., without rigorous
justification, application of Runge-Kutta formulas to Eq. (8.6.2), similar to the direct
introduction of trapezoidal formulas in Section 5.5.4. This is direct application of an ODE
integrator to equations that are not ODE and, in the present case, not even equivalent to an ODE.
For the second order ODE of Eq. (4.8.13),
v = g ( t, v , v& )
&& (8.6.7)
638
and the Runge-Kutta formula of Eq. (4.8.27) is
æ i i ö
k i = g ç t i + ci h,vn + hci v& n + h 2 å Ai jk j , v& n + h å a i jk j ÷ (8.6.8)
è j=1 j=1 ø
Defining intermediate variables vi and v& i (Petzold, 1986)

i
vi º v n + hci v& n + å Ai jk j
j=1
i
(8.6.9)
v& i º v& n + h å a i jk j
j=1
Eq. (8.6.8) is written as

ki = g(tn + ci h, vi , v& i ) (8.6.10)
Using this formal manipulation, Eq. (8.6.2) is discretized as
Rn º M(qi )D(qi ,t n + ci h)k i + FTq (qi ,t n + ci h)λi
(8.6.11)
-M(qi )UB(qi ,t n + ci h)g(qi , q& i ) - S(qi , q& i ) - Q (t n + ci h,qi , q& i , λi ,μ) = 0
A
where Eqs. (5.2.17) and (5.2.25) define qi and q& i as functions of vi and v& i ,
q i = q 0 + Vv i - Uh ( v i , t n + c i h)
(8.6.12)
q& i = D (q i , t n + c i h) v& i - UB (q i , t n + c i h)F t ( q i , t n + c i h)
With Eqs. (8.6.9) and (8.6.12), Eq.(8.6.11) is a system of equations in the unknown k i that may
be solved numerically, i = 1,...,s. The approximate solution of Eq. (8.6.2) is, from Eq. (4.8.28),
s
v n +1 = v n + hv& n + h 2 å Bjk j
j=1
s
(8.6.13)
v& n +1 = v& n + h å b jk j
j=1
As in the Index 0 DAE formulation of Section 5.5, the Jacobian of the residual of
Eq. (8.6.11) is J RK
n = éë¶R n / ¶&&
v Φ qT ùû , where ¶R n / ¶&&
v is given by Eqs. (5.5.34) and (5.5.17) and
the iterative solution algorithm is
éVk ij ù
J RK
n ê jú
= -R (k ij , l ij )
ëVl i û
j=1,2,L until R £ intol (8.6.14)
ék ij+1 ù ék ij ù éVk ij ù
ê j+1 ú = ê j ú + ê j ú
ë l i û ë l i û ëVl i û
for stages i = 1, L ,s. The solution for v n and v& n is given by Eqs. (8.6.13) and for q n and its
derivatives by Eqs. (5.2.33) through (5.2.35).
639
Finally, it is noted that with Eqs. (8.6.9) and (8.6.13) replaced by Eqs. (4.8.40) and with
only a single stage equation of Eq. (8.6.11) at t n +1 , the trapezoidal method is obtained.
For a given application, a multibody model is defined by generalized coordinates
L q Tnb ùû ; kinematic constraints of Eqs. (8.6.3); constraint Jacobian Fq (q, t) and
T
q = éë q T
1
derivative terms P 2 ( q , χ), P 3 (q , q& ), and P 4 (q , λ ) ; kinetic terms M ( q ) , Q (t, q, q& , λ, μ) , and
A
S ( q , q& ) and their derivatives M 2 ( q , μ ) , Qq (t, q, q& , λ, μ), Qq& (t, q, q& , λ, μ), Qλ (t, q, q& , λ, μ) ,
A A A
Sq (q, q& ), and Sq& (q, q& ) ; and coefficients of friction in the friction model of Section 8.1.2.2.
The numerical solution process, based on the Index 0 DAE formulation of Section 5.5, is
the implicit numerical integration algorithm with friction, as follows
(1) Define initial conditions q and q& at t 0 that satisfy the first two of Eqs. (8.6.3).
0 0
Evaluate matrices U and V that define the tangent space in the Index 0 DAE formulation of
Section 5.5 and initial conditions on independent tangent space generalized coordinates v 0
and v& 0 .
(2) At time step n, with known values of q and q& and estimates q = q and q& = q& , evaluate
n -1 n -1 n n
kinetic terms M (q ) , Q (t, q, q& , λ, μ) , and S (q , q& ) and their derivatives with respect to q, q& , and
A
λ. Evaluate derivatives ( ΦqT (q)λˆ )q , P 2(q , χ ) = ( Φ q (q)χˆ ) , and P3(q, q& ) = ( P2(q, q&ˆ )qˆ& )q of
q
constraints in Appendix 5.B. Apply an implicit Runge-Kutta or trapezoidal numerical

integrator to the Index 0 DAE to determine k n , vn and v& n , and evaluate solutions qn , q& n , and
&&qn using Eqs. (5.2.33) through (5.2.35). In the first time step, the parameterization of
coefficients of friction with variable w presented in Section 8.8.1 is used to obtain initial
conditions &&
v 0 and λ 0 .
(3) Monitor the condition number of the Jacobian J n , the number of Newton-Raphson
iterations required in Step 2, the magnitude of v, and the number of iterations required to
evaluate tangent space integration variables. If tolerances are exceeded, define a new time
t0 and associated q0 and repeat calculations in Step 1 to define a new tangent space
parameterization and initial conditions v 0 and v& 0 . Otherwise, continue with the same
parameterization.
(4) Continue the process until the final time tf is reached or a singularity is encountered.
Numerical integration of the Index 0 DAE of motion for systems with friction introduces
challenges that are not encountered in systems without friction. First, the equations cannot be
reduced to an ODE. Second, special care must be taken to determine initial values of
accelerations and Lagrange multipliers that satisfy the equations of motion at the initial time,
for use in iterative solution of the discretized equations of motion.
640
8.7 Code 8.7 for Simulation of Planar Systems with Friction
The Index 0 DAE components of planar tangent space multibody simulation Code 5.7 of
Appendix 5.A are extended to account for friction in revolute and translational joints, using the
formulation and derivatives presented in Section 8.4 and the solution algorithms of Section 8.6.
The rudimentary general-purpose MATLAB computer Code 8.7 of Appendix 8.A implements
the tangent space Index 0 DAE formulations of Sections 5.2 and 5.5 for planar multibody
systems, including the effects of friction in revolute and translational joints. All aspects of planar
systems incorporated in Code 5.7 are included, with friction generalized forces and their
derivatives of Section 8.4 and explicit and implicit numerical integration algorithms of Section
8.6 added. Explicit fixed time step Nystrom4 and variable time step RKFN45 algorithms and
implicit variable time step trapezoidal and SDIRK54 algorithms are implemented for numerical
integration of tangent space Index 0 DAE of planar system dynamics with friction.
Following an explanation of MATLAB Code 8.7 in this section, numerical examples are
presented in Section 8.8, including those treated in Section 5.8 without friction. To the author’s
knowledge, the friction representation presented in this chapter and included in Code 8.5 is not
yet available in commercial dynamic simulation software.
Components of MATLAB Code 8.7 that interface with the user, with the exception of the
AppData function that includes definition of friction parameters, are identical to Code 5.7 and
are presented in Section 5.7.1. Following a brief explanation of extensions of the AppData
function, an outline of the body of the code, with which the user need not interact, is presented in
Section 8.7.2.
Lines 14 through 23 that define the Joint Data Table in the AppData function of Fig.
5.7.3 for the three body translating model are modified to include friction data in Fig. 8.7.1. As
defined in line 15 of Fig. 8.7.1, three elements are added to define friction data, the radius R of
revolute joints and static and dynamic coefficients of friction ms = mus and m d = mud,
respectively. For the translational joint, the length of the key in body i that is d i in Eq. (8.4.17)
of the friction model is the length of vector v¢i = vipr that defines orientation of the joint in body
i. For the three body translating model the AppData data set of Fig 8.7.1 includes the lengths of
keys in bodies 1, 2, and 3 in translational joints 1, 2, and 3 that are defined in lines 20 through
22. Data in any field that are not required may be entered as zeros; e.g., the radius R in a
translational joint in line 27.
4 if app==1 %Three Body Translating Model

9
10 ux=[1;0];
11 uy=[0;1];
12 z2=zeros(2,1);
13
641
15 %PJDT(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr;R;mus;mud;ms;nm];
17 %sipr&sjpr=vectors to Pi&Pj, d=dist., vipr&vjpr=vectors along trans axis,
18 %length of vi is di in Eq. (8.4.17), R=rad Rev, mus&mud=FrCoefs,
19 %ms=Lagrange muplt.start address, nm=no. of mujlt.
20 PJDT(:,1)=[2;1;0;z2;z2;0;0.1*ux;uy;0.1;0.5;0.3;1;2];%Tran-Bod1toGrd
21 PJDT(:,2)=[2;2;0;z2;z2;0;0.1*ux;ux;0.1;0.5;0.3;3;2];%Tran-Bod2toGrd
22 PJDT(:,3)=[2;3;0;z2;z2;0;0.1*ux;ux;0.1;0.5;0.3;5;2];%Tran-Bod3toGrd
23 PJDT(:,4)=[3;1;2;z2;z2;5;z2;z2;zeros(3,1);7;1]; %Dist.-Bod1 to Bod2
26 nv=ngc-nc;
27 nu=nc;
28
30 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
31 PMDT=[[6;1],[2;1],[2;1]];
32
37 PTSDAT(:,1)=[1;0;z2;-10*uy;10;0;10;0]; %Bod1 to Grnd
38 PTSDAT(:,2)=[2;3;z2;10*ux;10;0;11;0]; %Bod2 to bod3
39
45
47
48 q0=[0;0;pi/2;5*ux;0;6*ux;0];
49 qd0=[0;0;0;0;0;0;-1;0;0];
50
51 end
Figure 8.7.1 AppData Function, Three Body Translating Model
Lines 30 through 43 in the AppData function of Fig. 8.7.1 are code that define addresses
of Lagrange multipliers in the array LLam in Code 8.7. The user need not modify these lines of
code.
Figure 8.7.2 presents code in the main program that defines initial values of &&v = vdd and
λ = Lam that are required as initial estimates for iterative computation defined in Section 8.5.2,
for use in both explicit and implicit integration algorithms with friction. The user may modify the
value N in line 117 that defines the number of increments in Lagrange multipliers in the
transition from zero coefficients of friction to the specified values in the AppData function of
Fig. 8.7.1. The default value 10 has been found adequate for applications treated to date, but may
need to be increased if very large coefficients of friction are considered.
642
98 %Initial vdd and Lam Estimate with no friction
99 M=MEval(PMDT,par);
100 Gam=GamEval(0,q0,qd0,PJDT,par);
101 D=(eye(ngc)-U*B*Phiq)*V;
102 QA0=QAEval(0,q0,qd0,zeros(nc),PMDT,PJDT,LamInd,PTSDAT,PRSDAT,par);
103
104 EE=[M*D,Phiq'];
105 CondEE1=cond(EE);
106 RHS=M*U*B*Gam+QA0;
107 x=EE\RHS;
108 Pvdd=[eye(nv),zeros(nv,nc)];
109 PLam=[zeros(nc,nv),eye(nc)];
110 vdd=Pvdd*x;
111 Lam=PLam*x;
112 vdd00=vdd;
113 Lam00=Lam;
114
115 %Increment friction coefficients to obtain initial conditions on Lam and
116 %vdd
117 N=10;
118 w=1;
119 %QAwsLam=QAwsLamEval(q0,qd0,Lam,PJDT,LamInd,par,w,N);
120 while w<=N
121 [vdd,Lam,jodeiter,ECond]=FrODEfunct0w(q0,qd0,vdd,Lam,...
122 V,U,B,PJDT,LamInd,PMDT,PTSDAT,PRSDAT,par,w,N);
123 Econd0(w)=ECond;
124 jodeiter0(w)=jodeiter;
125 Vvdd0(:,w)=vdd;
126 LLam0(:,w)=Lam;
127
128 w=w+1;
129 end
130
131 qdd=D*vdd-U*B*Gam;
132 Qdd(:,1)=qdd;
C:\Users\echau\Documents\CMMD\Chapter 8. Simulation of F...\AA_Planar_Ind0_Multibody_Sim_Friction.m Page
3
133 Vvdd(:,1)=vdd;
134 LLam(:,1)=Lam;
Figure 8.7.2 Calculation of Initial Values of &&v = vdd and λ = Lam
Computational components of the main code are essentially identical to those presented
in Section 5.7.2, with two distinctions. First, only four integration options are supported for the
Index 0 DAE formulation, two explicit and two implicit. Second, functions Q A = QAEval and
Q qA and Q qA& in QAsqqdEval define generalized force and its derivatives with respect to
generalized coordinates that include the effects of friction that are defined in Section 8.4. In
addition, derivatives of generalized force with respect to Lagrange multipliers are defined in
function Q Aλ = QAsLamEval.
643
Code 8.7 is an extension of Code 5.7 that includes effects of friction in revolute and
translational joints. User supplied data are as in Code 5.7, with the exception of coefficients of
static and dynamic friction, the radius of the revolute joint, and the length of the key in the
translational joint. These data are provided in the AppData function.
Code that implements friction generalized forces defined in Section 8.4 is included in functions
QAEval, QAsqqdEval, and QAsLamEval that need not be modified by the user.
644
8.8 Planar System Simulation with Friction Using Code 8.7
Five planar systems that have previously been modeled without friction are simulated
with friction effects modeled. A key issue addressed is the presence of stiffness in the resulting
equations of motion, as has been suggested in the literature, based on use of Index 3 DAE in
models with friction (Marques, Flores, Pimenta Claro, and Lankarani, 2016; Pennestri, Rossi,
Salvini, and Valentini, 2016; Brown and McPhee, 2016). To resolve this issue, performance of
explicit numerical integration algorithms in simulating systems with friction is considered. If
explicit algorithms fail, the systems may indeed be stiff. If they succeed, the systems are clearly
not stiff.
8.8.1 Three-Slider Mechanism
The three-slider mechanism shown in Fig. 8.8.1 is comprised of translational joints
between each of the bodies and one of the axes in ground, a distance constraint between bodies
one and two, and springs between body one and ground and between bodies two and three. It is
identical to the model of Section 5.8.4 and similar to the three particle model with friction of
Section 8.1. The body fixed x  axis in each body is along the long dimension of the body.
Vectors that define the joint between body one and ground are v1 = u x and v0 = u y and vectors
that define the joints between bodies two and three and ground are v2 = v3 = u x and v0 = u x . The
length of the distance constraint is = 5 m , the lengths of keys in translational joints of bodies 1,
2, and 3 are 0.1 m, spring constants are k1 = k 2 = 10 N/m , masses and moments of inertia of the
bodies are m1 = 5 kg, m2 = m3 = 2 kg, and J1 = J 2 = J 3 = 1 kgm 2 , coefficients of friction are
s = 0.5 and d = 0.3 , and initial conditions are q1 =  0 0  / 2 , q 2 = 5 0 0 ,
T T
q3 = 6 0 0 , q1 = q 2 = 0 , and q3  −1 0 0 . The data set for simulation of this mechanism

T T
using Code 8.7 is presented in Fig. 8.8.2, which differs from that for use in Code 5.7, as
presented in Fig. 5.8.15, only by addition of friction data.
Figure 8.8.1 Three-Slider Mechanism
4 if app==1 %Three Body Translating Model

9
10 ux=[1;0];
11 uy=[0;1];
645
12 z2=zeros(2,1);
13
15 %PJTd(:,k)=[t;i;j;sipr;sjpr;d;vipr;vjpr;R;mus;mud;ms;nm];
17 %si&jpr=vectors to Pi&j, d=dist., vi&jpr=vectors along trans axis,
20 PJDT(:,1)=[2;1;0;z2;z2;0;0.1*ux;uy;0.1;0.5;0.3;1;2]; %Tran-Bod1 to ground
21 PJDT(:,2)=[2;2;0;z2;z2;0;0.1*ux;ux;0.1;0.5;0.3;3;2]; %Tran-Bod2 to ground
22 PJDT(:,3)=[2;3;0;z2;z2;0;0.1*ux;ux;0.1;0.5;0.3;5;2]; %Tran-Bod3 to ground
23 PJDT(:,4)=[3;1;2;z2;z2;5;z2;z2;zeros(3,1);7;1]; %Dist.-Bod1 to Bod2
26 nv=ngc-nc;
27 nu=nc;
28
30 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
31 PMDT=[[6;1],[2;1],[2;1]];
32
36 %C=damping coefficient,el0=spring free length, F=const. force
37 PTSDAT(:,1)=[1;0;z2;-10*uy;10;0;10;0]; %Bod1 to Grnd
38 PTSDAT(:,2)=[2;3;z2;10*ux;10;0;11;0]; %Bod2 to bod3
39
43 %C=damping coefficient,phi0=spring free angle, T=constant torque
45
47
48 q0=[0;0;pi/2;5*ux;0;6*ux;0];
49 qd0=[0;0;0;0;0;0;-1;0;0];
50
51 end
Figure 8.8.2 AppData Function, Three Slider Mechanism
Plots of position and velocity of each of the bodies predicted by Code 8.7 with the
foregoing data are presented in Fig. 8.8.3. Note the delay in onset of stiction in this model, as
occurred in the model problem of Section 8.1, Fig. 8.1.5. With an increased mass of body one to
m1 = 6kg , Fig. 8.8.4 shows that stiction occurs at the first possible time, as in Fig. 8.1.7 of
Section 8.1. The magnitudes of mass that trigger the distinct modes of stiction response are
different than in the particle model, as expected due to the fundamental difference in the particle
and rigid body system models. Nevertheless, the qualitative behavior is identical.
646
Figure 8.8.3 Position and Velocity, m1 = 5 kg
Figure 8.8.4 Position and Velocity, m1 = 6 kg

As a check on validity of solutions obtained with the trapezoidal integration algorithm,
maximum norms of position, velocity, and acceleration constraint errors were 5e-15, 1.2e-14,
and 7e-14, respectively. Comparable results were obtained using the SDIRK54 implicit
integration algorithm and the Nystrom4 and RKFN45 explicit integration algorithms. Success in
simulation with the explicit integration algorithms shows that the system is not stiff.
8.8.2 Slider-Crank
Dynamics of the two body model of the slider-crank mechanism of Fig. 8.8.5 that was
analyzed kinematically in Section 3.2.3.3 and dynamically in Section 5.8.5 is analyzed here with
the addition of friction. The radius of the crank is 1 m, masses and moments of inertia of bodies
one and two are m1 = 5 kg , J1 = 5 kgm 2 , m 2 = 1 kg , and J 2 = 1 kgm 2 , coefficients of friction are
s = 0.25 and d = 0.2 , and the length of the key in the translational joint and the radius of the
= 1.5 m are q1 =  0 0 0 ,
T
revolute joint are 0.1 m. Initial conditions with
q 2 =  2.5 0 0 , q1 =  0 0 100 , and q 2 =  0 0 0 . The data set for simulation of this
T T T
mechanism with Code 8.7 is presented in Fig. 8.8.6.
647
Figure 8.8.5 Slider-Crank
55
60
61 ux=[1;0];
62 uy=[0;1];
63 z2=zeros(2,1);
64
71 PJDT(:,1)=[1;1;0;z2;z2;0;z2;z2;0.1;0.25;0.2;1;2]; %Revolute-crank to ground
72 PJDT(:,2)=[2;2;0;z2;z2;0;0.1*ux;ux;0.1;0.25;0.2;3;2]; %Trans.-slider2 to ground
73 PJDT(:,3)=[3;1;2;ux;z2;1.25;z2;z2;zeros(3,1);5;1]; %Dist.-crank to slider2
75 nc=5; %Number of holonomic constraint equations
78
80 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
81 PMDT=[[5;5],[1;1]];
82
88
94
648
96
97 q0=[0;0;0;2.25*ux;0];
98 qd0=[0;0;100;0;0;0];
99
100 end
Figure 8.8.6 AppData Function, Two Body Slider-Crank
Plots of angular acceleration of the crank (omegd) and total energy divided by 20 (TE/20)
vs time for the basic connecting rod length (el) 1.5 m and for 1.25 m and 1.15 m, obtained using
Code 8.7 are presented in Fig. 8.8.7. Angular acceleration, hence constraint force, increases as
connecting rod length is decreased toward the singular configuration = 1 . Accordingly, kinetic
and total energy decrease 56%, 71%, and 79%, respectively, over the 0.5 sec simulation.
Figure 8.8.7 Crank Angular Acceleration and Total Energy/20 vs Time

As a check on validity of solutions obtained with the trapezoidal integration algorithm,
maximum norms of position, velocity, and acceleration constraint errors are 4e-15, 1e-12, and
7e-10, respectively. Comparable results are obtained using the SDIRK54 implicit integration
algorithm and the RKFN45 explicit integration algorithms. The constant step size Nystrom4
integration algorithm failed in all three simulations, likely due to the extreme accelerations
involved. Success in simulation with the explicit RKFN45 integration algorithm shows that the
system is not stiff.
649
The quick return mechanism shown in Fig. 5.8.6 of Section 5.8.2 is a somewhat more
complicated mechanism that is based on revolute, translational, and distance constraints. It
undergoes large acceleration, hence large constraint forces that lead to significant friction effects.
Data for the mechanism are as presented in Section 5.8.2. Rather than simulating the system with
no external input and observing decay of motion, an RSDA is imposed on the crank with an
applied 10,000 Nm torque that was arrived at to maintain approximately periodic motion, in the
presence of friction coefficients s = 0.3 and d = 0.25 in all joints. The AppData function for
simulation of the system, using Code 8.7 is provided in Fig. 8.8.8.

104
108 NRSDA=1; %Number of RSDA torque elements
109
110 ux=[1;0];
111 uy=[0;1];
112 z2=zeros(2,1);
113
120 PJDT(:,1)=[1;1;0;-2*ux;z2;0;z2;z2;0.1;0.3;0.25;1;2]; %Revolute-bar to ground
121 PJDT(:,2)=[1;2;0;z2;2*uy;0;z2;z2;0.1;0.3;0.25;3;2]; %Revolute-crank to ground
122 PJDT(:,3)=[1;2;3;1.5*ux;z2;0;z2;z2;0.1;0.3;0.25;5;2]; %Revolute-crank to key
123 PJDT(:,4)=[2;3;1;z2;z2;0;0.2*ux;ux;0;0.3;0.25;7;2]; %Trans.-bar to key
124 PJDT(:,5)=[2;4;0;z2;4*uy;0;0.1*ux;ux;0;0.3;0.25;9;2]; %Trans.-cutter to ground
125 PJDT(:,6)=[3;1;4;2*ux;z2;2.5298;z2;z2;zeros(3,1);11;1]; %Dist.-bar to cutter
126
131
133 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
134 PMDT=[[100;100],[1000;1000],[1;1],[50;50]];
135
138 %i&j=bodies connected, si&jpr=vectors to Pi&j, K=spring constant,
141
650
143 %PRSDAT(:,R)=[i;j;K;C;phi0;T]; R=RSDA No.,
146 PRSDAT(:,1)=[2;0;0;0;0;5000];
147
149 q10=[1.2;1.6;0.9273];
150 q20=[0;2;0];
151 q30=[1.5;2;0.9273];
152 q40=[0;4;0];
153
154 q0=[q10;q20;q30;q40];
156
157 end
Figure 8.8.8 AppData Function, Quick Return Mechanism
Plots of the horizontal coordinate (x4) of the tool and angular velocity (omeg2) of the
crank in the lower graph of Fig. 8.8.9 show that near steady state motion is achieved. The upper
graph of the figure represents response to a torque of 5,000 Nm, which leads to a loss of 31% in
kinetic energy over the 5 second simulation, 26% of the loss occurring in the first 2 seconds.
Maximum norms of position, velocity, and acceleration constraint errors in these simulations,
using the trapezoidal integration algorithm, were 5.5e-15, 7e-14, and 1.5e-12, respectively.
Comparable results were obtained using the SDIRK54 implicit integration algorithm and the
Nystrom4 and RKFN45 explicit integration algorithms, confirming that the system is not stiff.
651
Figure 8.8.9 Motion of Crank and Body 4 with Varying Applied Torque
8.8.4 Surge Waves in a Coil Spring
The lumped mass spring model of Section 5.8.3, dividing the spring into 5 equal
lengths 0.2 m, each with stiffness k = 5K, and approximating the mass as 5 lumped
masses, m = M/5 each, is modified to include friction as the spring slides in a horizontal
guide. For computation, M = 1 kg, K = 1000 N/m, the spring has length L = 1 m, and
coefficients of friction s = 0.2 and d = 0.19 act between the spring masses and the
horizontal guide. The initial position of mass i is x i = 0.2i m, i = 1, ... 5. To study the
effect of friction on surge wave behavior, motion of the system is initiated by giving
mass 5 an initial velocity 1 m/sec in the negative x direction, which is imparted by
impact with another body. The initial velocity of the first four masses is zero.
Kinematics of the system is defined by prescribing a translational joint between
each of the masses and ground. Each spring is attached at the centers of mass of the
bodies it connects. The mass of each body is m = 0.2 kg, and each spring has spring rate
k = 5000 N/m and free length 0 = 0.2 m. Data for simulation of the system with Code
8.7 are given in the AppData Function in Fig. 8.8.10.
159 if app==4 %Lumped Mass Coil Spring-5 masses

160
652
169
176 PJDT(:,1)=[2;1;0;z2;z2;0;0.1*ux;ux;0;0.2;0.19;1;2]; %Tran.-Body 1 to Ground
181
184 nv=ngc-nc;
185 nu=nc;
186
187 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]] Mass Data Table
188 PMDT(:,1)=[0.2;0.1];
189 PMDT(:,2)=[0.2;0.1];
190 PMDT(:,3)=[0.2;0.1];
191 PMDT(:,4)=[0.2;0.1];
192 PMDT(:,5)=[0.2;0.1];
193
194 %PTSDAT(10,NTSDA) Planar TSDA Data Table
198 PTSDAT(:,1)=[1;0;z2;z2;5000;0;0.2;0]; %Body 1 to Ground
199 PTSDAT(:,2)=[1;2;z2;z2;5000;0;0.2;0]; %Body 1 to 2
200 PTSDAT(:,3)=[2;3;z2;z2;5000;0;0.2;0]; %Body 2 to 3
201 PTSDAT(:,4)=[3;4;z2;z2;5000;0;0.2;0]; %Body 3 to 4
202 PTSDAT(:,5)=[4;5;z2;z2;5000;0;0.2;0]; %Body 4 to 5
203
209
211
212 q0=[0.2;0;0;0.4;0;0;0.6;0;0;0.8;0;0;1;0;0];
213 qd0=[zeros(12,1);-1;0;0];
Figure 8.8.10 AppData Function, Five Lumped Mass Coil Spring Model
Numerical results of dynamic simulation carried out with Code 8.7 include plots
of displacements delxi= x i − x i0 , i = 1,...,5, versus time for each lumped mass in Fig.
8.8.11. In comparison with results of Fig. 5.8.12 that were obtained for the same system
653
without friction, there is a loss of 40% in total energy in a single pass of the wave, and
the sharpness of wave behavior is diminished.

The disk (body 1) in Fig. 8.8.12 rotates in ground (body 0) about the origin of
the x-y global frame. Body 2 translates in body 1 along a guide that is parallel to the
body fixed y1 axis, one unit to the right of the y1 axis. A TSDA with spring constant K
= 10 N/m, damping constant C = 0, and free length of 10.1 m acts between the origin of
the x 2 -y2 reference frame and a point u1x + 10u1y in body 1. Static and dynamic
coefficients are s = 0.06 and s = 0.05 , respectively, and masses and moments of
654
inertia of the bodies are m1 = 10 kg, J1 = 10 kg  m2 , m2 = 5 kg, and J1 = 5 kg  m2 . Initial
conditions for the system are q10 =  0 0 0 , q10 =  0 0 0 , q 02 = 1 0 0 , and
T T T
q 02 =  0 0 0 . To simulate motion in a horizontal plane, the acceleration due to

T
gravity is set to zero; i.e., g = 0.

The AppData function for simulation of this system using Code 8.7 is shown in
Fig. 8.8.13. Simulations are carried out over a 10 sec interval, with plots of rotation of
body 1, 1 = phi1, and translation of body 2 relative to body 1, y2 = dely2pr, shown in
Figs. 8.8.14 and 8.8.15. Identical results were obtained with all four integration
formulations. For the smaller damping coefficients in Fig. 8.8.14, total energy decreased
by 96% over the simulation and kinetic energy at the end of the simulation was 7.3e-4
Nm. For the larger damping coefficients in Fig. 8.8.15, total energy decreased by 99.7%
and kinetic energy at the end was 9.8e-5. For larger coefficients of friction, kinetic
energy reached near zero before the end of the simulation and integrators were unable to
continue the simulation.

219
224
225 ux=[1;0];
226 uy=[0;1];
227 z2=zeros(2,1);
228
235 PJDT(:,1)=[1;1;0;z2;z2;0;z2;z2;0.1;0.8;0.07;1;2]; %Revolute-1 to ground
655
236 PJDT(:,2)=[2;1;2;ux;z2;0;0.1*uy;uy;0.1;0.8;0.07;3;2]; %Trans.-1 to 2
237
242
244 %PMDT=[[m1;J1],[m2;J2],...,[mnb;Jnb]]
245 PMDT=[[10;10],[5;5]];
246
251 PTSDAT(:,1)=[1;2;ux+10*uy;z2;10;0;10.1;0];
252
258
260
261 q0=[0;0;0;ux;0];
262 qd0=[0;0;0;0;0;0];
263
264 end
Figure 8.8.13 AppData Function, Rotating Disk with Translating Body
Figure 8.8.14 1 = phi1 and y2 = dely2pr, s = 0.05 and d = 0.04
656
Figure 8.8.15 1 = phi1 and y2 = dely2pr, s = 0.08 and d = 0.07
The Index 0 DAE formulation and numerical integration algorithms presented for planar
systems in Sections 8.4 and 8.6, implemented in Code 8.7, are shown to be effective in
simulation of planar systems with friction. Of particular importance is the demonstrated ability
of explicit integrators to solve the equations of motion. This demonstrates that friction effects
do not necessarily imply that the resulting equations of multibody dynamics are stiff.
657
8.9 Code 8.9 for Simulation of Spatial Systems with Friction
The Index 0 DAE components of spatial tangent space multibody simulation Code 5.9 of
Appendix 5.A are extended to account for friction in cylindrical, revolute, and translational
joints, using the formulation and derivatives presented in Section 8.5 and the solution algorithms
of Section 8.6. The rudimentary general-purpose MATLAB computer Code 8.9 of Appendix 8.A
implements the tangent space Index 0 DAE formulations of Section 5.5 for spatial multibody
systems, including the effects of friction in cylindrical, revolute, and translational joints. All
aspects of spatial systems incorporated in Code 5.9 are included, with friction generalized forces
and their derivatives of Section 8.5 and explicit and implicit numerical integration algorithms of
Section 8.6 added. Explicit fixed time step Nystrom4 and variable time step RKFN45 algorithms
and implicit variable time step trapezoidal and SDIRK54 algorithms are implemented for
numerical integration of tangent space Index 0 DAE of spatial system dynamics with friction.
Following an explanation of MATLAB Code 8.9 in this section, numerical examples are
presented in Section 8.10, including those treated in Section 5.10, with friction effects added. To
the author’s knowledge, the friction representation presented in this chapter and included in Code
8.9 is not yet available in commercial dynamic simulation software.
Components of MATLAB Code 8.9 that interface with the user, with the exception of the
AppData function that includes definition of friction related parameters, are identical to Code 5.9
and are presented in Section 5.9.1. Following a brief explanation of extensions of the AppData
function, an outline of the body of the code, with which the user need not interact, is presented in
Section 8.9.2.
Lines 17 through 24 that define the Joint Data Table in the AppData function of Fig.
8.9.1 for the One Body Cylindrical Joint with Ground of Fig. 5.10.5 are modified from Fig.
5.10.6 to include friction related data. As defined in lines 17 through 23 of Fig. 8.9.1, six
elements of data are added to define friction; radius R of revolute joint, dimension b of the
translational joint, static and dynamic coefficients of friction ms = mus and m d = mud, and
parameters ms and nm that define the address of the first Lagrange multiplier associated with the
joint and the number of constraint equations for the joint, respectively. The last two parameters
are used in friction force calculations to select the Lagrange multipliers that determine friction
forces for the joint. Data in any field that are not required may be entered as zeros.
10 if app==1 %Body in Cylindrical Joint with Ground

11
14
16
18 %SJTd(:,k)=[t;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr;a;b or R;mus;mud;mc;nm];
21 %si&jpr=vectors to Pi&j; d=dist.; vxipr, vzipr, vxjpr, vzjpr;
22 %a,b or R=joint dimensions; mus,mud=coeffidients of stat. &dyn. frict.
658
23 %ms=address of firsrt Lag. Mult,;nm= No. of Constr. Eq.
24 SJDT(:,1)=[3;1;0;z3;z3;0;ux;uz;ux;uz;0.1;0.05;0.24;0.2;1;4];%Cyl - Body1 to Ground
25
29 nv=ngc-nc;
30 nu=nc;
31
34 SMDT=[[1;0.1;0.1;0.1]];
35
37 if NTSDA==0
39 end
43 STSDAT(:,1)=[1;0;uy+uz;-ux+uy+uz;10;0;1;0];
44
46 r10=[0;0;0];
47 p10=[1;0;0;0];
48 q0=[r10;p10];
49 r1d0=[0;0;0];
50 p1d0=0.5*EEval(p10)'*10*uz;
51 qd0=[r1d0;p1d0];
52
53 end
Figure 8.9.1 Friction in AppData Function for Body in Cylindrical Joint with Ground
Figure 8.9.2 presents code in the main program and function Ind0EC that defines initial
v = vdd and λ = Lam that are required as initial estimates for iterative computation
values of &&
defined in Section 8.5.2, for use in both explicit and implicit integration algorithms with friction.
The user may modify the value N in line 117 that defines the number of increments in Lagrange
multipliers in the transition from zero coefficients of friction to the specified values in the
AppData function of Fig. 8.9.1. The default value 10 has been found adequate for applications
treated to date, but may need to be increased if very large coefficients of friction are considered.
83 %Calculation of initial values of qdd and Lam to start integration
84 %Coefficients of friction are Indexed by w/N, w=0,1,...N in Function
85 %Ind0IC to calculate a sequence of qdd and Lam that converge to the
86 %desired coefficients of friction. The user may select N.
88 N=10;
89 [qdd,Lam,Qdd0,LLam0,w]=Ind0IC(q0,qd0,SMDT,SJDT,STSDAT,par,N);
91 Qdd(:,1)=qdd;
92 LLam(:,1)=Lam;
1 function [qdd,Lam,Qdd0,LLam0,w]=Ind0IC(q0,qd0,SMDT,SJDT,STSDAT,par,N)
659
3 [nb,ngc,nh,nc,nv,nu,g,utol,Btol,intol,Atol,Vtol,hvar,NTSDA,vt]=...
4 parPart(par);
5
6 %Evaluate Initial Acceleration and Lagrange Multipliers;
8 %Initial Parameterization
9 Phiq=PhiqEval(0,q0,SJDT,par);
10 U=Phiq';
11 B=inv(U'*U);
12 V=null(U');
13 v=zeros(nv,1);
14 vd=V'*qd0;
15 u=zeros(nu,1);
16 Vv(:,1)=v;
17 Vvd(:,1)=vd;
18 Uu(:,1)=u;
19
20 %Increment friction coefficients to obtain initial conditions on Lam and
21 %vdd
22 w=0;
23 Lam=zeros(nc,1);
24 vdd=zeros(nv,1);
25 while w<=N
26 [vdd,Lam,jodeiter,ECond]=FrODEfunct0w(q0,qd0,vdd,Lam,...
27 V,U,B,SJDT,SMDT,STSDAT,par,w,N);
29 w=w+1;
31 Gam=GamEval(0,q0,qd0,SJDT,par);
32 qdd=V*vdd-U*B*Gam;
33 Econd0(w)=ECond;
34 jodeiter0(w)=jodeiter;
35 Qdd0(:,w)=qdd;
36 LLam0(:,w)=Lam;
37
38 end
v = vdd and λ = Lam
Figure 8.9.2 Calculation of Initial Values of &&
Computational components of the main code are essentially identical to those presented
in Section 5.9.2, with two distinctions. First, only four integration options are supported for the
Index 0 formulation. Second, functions Q A = QAEval and QqA and QqA& in QAsqqdEval define
generalized force and its derivatives with respect to generalized coordinates that include the
effects of friction defined in Section 8.5. In addition, derivatives of generalized force with
respect to Lagrange multipliers are defined in function Q Aλ = QAsLamEva.
660
Code 8.9 is an extension of Code 5.9 that includes the effects of friction in cylindrical,
revolute, and translational joints. User supplied data are as in Code 5.9, with the exception of
coefficients of static and dynamic friction, the radius of the revolute joint, and the distance b in
the translational joint. These data are provided in the AppData function.
Code that implements friction generalized forces that are defined in Section 8.5 is included in
functions QAEval, QAsqqdEval, and QAsLamEval that need not be modified by the user.
661
8.10 Spatial System Simulation with Friction Using Code 8.9
Four spatial constrained systems are modeled using Code 8.9 of Appendix 8.A. Stiction is
shown to occur in some examples.
8.10.1 Body in Cylindrical Joint with Ground
A single body i = 1 shown in Fig. 8.10.1 is constrained to rotate and translate in a
cylindrical joint, with axis along the global z axis in ground body j = 0. A spring connects point
A (0,1,1) on the body with point B ( - 1,1,1) in ground. With the joint reference frame, initially at
the origin of and aligned with the global x-y-z frame, s¢0 = s1¢ = 0 and C0 = C1 = I . With ground
1 1 1 1
fixed in space, r0 = 0 and p0 = [1 0 0 0] . The initial position and orientation of body 1 are
T
r1 (0) = 0 and p1 (0) = [1 0 0 0] . The dimensions of the cylindrical joint are

T
a 1 = 0.1 m and R 1 = 0.05 m , the mass and moments of inertia of body 1 are 1 kg and 0.1 kg × m ,
2
gravitational acceleration is 9.8 m/ sec 2 in the negative z direction, the free length of the spring is
l0 =1, and the spring constant is k = 10 N/m. Coefficients of translational and rotational friction
are μdt = μdr = 0.2 and μst = μsr = 0.24 . The AppData function in Code 8.9 for this system is
presented in Fig. 8.9.1.
Figure 8.10.1 Body in Cylindrical Joint with Ground

Simulations are carried out with all four integration methods, using the initial
configuration shown in Fig. 8.10.1, r&1 (0) = 0 , angular velocity omega(0) = 10 rad/sec, an
integration step size of h = 0.0001 sec, and vt = 20h in Eq. (8.2.10). All four integration
methods yielded essentially identical results. Using the trapezoidal implicit integrator, constraint
reaction force components of Eq. (8.5.4), with fp º f ¢ , are shown in Fig. 8.10.2. Vertical friction
force and angular friction torque of Eqs. (8.5.12) and (8.5.15) are shown in Fig. 8.10.3. Vertical
and angular velocity of the body are shown in Fig. 8.10.4. It is interesting to note that, even
though the friction forces shown in Fig. 8.10.3 are approximately discontinuous, due to changes
in sign of slip velocities, the constraint contact forces of Fig. 8.10.2 that lead to the friction
forces are continuous.
662
Figure 8.10.2 Components of Constraint Contact Force
Figure 8.10.3 Vertical Friction Force and Friction Torque

What appear to be severe discontinuities in friction force and torque in Fig. 8.10.3 are in
fact continuous when viewed on an expanded time scale. These forces are represented by the
continuous friction model. As in model problems studied in Sections 8.1 through 8.3, the
continuous formulation provides an effective method for modeling and simulation of dynamics
with friction and stiction.
Another interesting behavior is the three periods of stiction in the vertical direction,
shown by the plot of vertical velocity z& 1 = z1d in Fig. 8.10.4, corresponding to periods of
stiction force equal to the weight (9.8 N) of the body in Fig. 8.10.3. No stiction occurs in angular
motion, which is possible with the cylindrical bearing model presented in Section 3.3.3 that
decouples translational and rotational motion. Kinetic coupling of rotational and angular motion
occurs as a result of interaction induced by the spring. If a design based on point contact at each
end of the cylindrical sleeve in body i were adopted, friction forces would be coupled and this
form of stiction could not occur.
663
Figure 8.10.4 Vertical and Angular Velocity of Body 1
As a check on accuracy of simulation results, the equations of motion were satisfied to
within an error tolerance of e-6 and maximum position, velocity, and acceleration constraint
error norms over the simulation period were 1.5e-11, 2.5e-10, and e-7, respectively.
To see if the equations of motion with friction are stiff, the simulation is run with the
explicit Nystrom4 integrator and a step size h = 0.001. The condition number of the ODE solver
in explicit integration and the Jacobian in implicit integration varied between 6 and 15, and only
one reparameterization was required in 10,000 time steps. Successful simulation with the explicit
Nystrom4 integrator shows conclusively that there is no stiffness in the Index 0 DAE of motion,
unlike indications of stiffness reported in the literature for the Index 3 DAE formulation with
friction (Marques, Flores, Pimenta Claro, and Lankarani, 2016; Pennestri, Rossi, Salvini, and
Velentini, 2016; Brown and McPhee, 2016). This is likely due to the fact that integrators in the
Index 0 DAE formulation are applied to just two independent generalized coordinates, whereas
Index 3 DAE integrators apply ODE integration formulas to all seven generalized coordinates.
8.10.2 Slider-Crank
The spatial slider-crank mechanism shown in Fig. 8.10.5 has been used as a
computational example in kinematics and dynamics without friction in Sections 3.3.8.2 and
3.7.8.3 and with friction (Marques, Flores, Pimenta Claro, and Lankarani, 2016). It involves
potentially large constraint contact forces and near singular configurations that may be
dominated by friction effects. Body 1 rotates about an axis parallel to and offset from the global z
axis in ground (body 0), with a crank in the x-y plane of radius 0.08 m. The slider (body 2)
translates in a guide along the global z axis in ground. It has an offset of length c 2 in the positive
y2 direction for distance constraint attachment, which increases constraint reaction forces in the
translational joint. A distance constraint of length d 3 connects the top of the offset in body 2
with the end of the crank in body 1. Data are given in the mks system of units.
664
As in Section 5.10.4, since the x ¢-y ¢-z ¢ reference frames in both bodies and ground are
parallel, unit vectors used in definition of the joints are v¢x = u z , v¢yik = u y , and v¢z = ux , i = 0, 1,
ik ik
2 and k = 1 and 2. For the revolute joint, s¢1 = 0 , s¢01 = [ - 1 0 .1 0 .12 ] , R = 0.1,
1 T
μ s = 0.1, and μ d = 0.08 . For the translational joint, s¢2 = s¢0 = 0 , μ s = 0, and μ d = 0 . For the
2 2
distance constraint, s1¢ = 0.08uz , c2 = 0.02 , s¢23 = c 2u y , and d 3 = 0.4 . Masses and moments of
3
inertia are m1 = 0.5 and J1 = 0.2I3 for body one and m2 = 5 and J 2 = 0.2I 3 for body two. Finally,
initial conditions are r10 = [0, 0.1, 0.12]T , r20 = [0.3429, 0, 0]T , p10 = p02 = [1, 0, 0, 0]T , r&10 = [0, 0, 0]T ,
r&20 = [2.799,0, 0]T , and initial angular velocity of body 1 about the x axis is 120 rad/sec, so
p& 10 = 0.5E(p10 )T120u x and p& 02 = [0, 0, 0, 0]T . The values of z 2 and z& 2 are estimated and initial
simulations run for tf = 0.002, adjusting the values until the variable arrays Q and Qd are
consistent with initial conditions. Data for simulation using Code 8.9 are presented in the
55 if app==2 %Spatial Slider-Crank

56
60
62 %SJDT(:,k)=[t;i;j;sipr;sjpr;d;vxipr;vzipr;vxjpr;vzjpr;a;b or R;mus;mud;mc;nm];
68 SJDT(:,1)=[4;1;0;z3;0.1*uy+0.12*uz;0;uz;ux;uz;ux;0.1;0.1;0.1;0.08;1;5]; %Rev-Body1 to Ground
69 SJDT(:,2)=[5;2;0;z3;z3;0;uz;ux;uz;ux;0.1;0.1;0;0;6;5]; %Tran-Body2 to Ground
70 SJDT(:,3)=[1;1;2;0.08*uz;0.02*uz;0.3;z3;z3;z3;z3;0.1;0.1;0.1;0.08;11;1];%Dist-Body 1 to 2
71
665
75 nv=ngc-nc;
76 nu=nc;
77
80 SMDT=[[0.5;0.2;0.2;0.2],[5;0.2;0.2;0.2]];
81
83 if NTSDA==0
85 end
89
91 r10=[0;0.1;0.12];
92 p10=[1;0;0;0];
93 r20=[0.2182;0;0];
94 p20=[1;0;0;0];
95 q0=[r10;p10;r20;p20];
96 r1d0=[0;0;0];
97 p1d0=0.5*EEval(p10)'*120*ux;
98 r2d0=[4.406;0;0];
99 p2d0=[0;0;0;0];
100 qd0=[r1d0;p1d0;r2d0;p2d0];
101
102 end
Numerical results for angular velocity omega1x and angular acceleration omega1xd of
the crank for the data set in Fig. 8.10.6 with d 3 = 0.3 (with x 02 = 0.2182 and x& 02 = 4.406 ) are
shown in the bottom plots of Fig. 8.10.7. The effect of the relatively massive slider on angular
velocity and acceleration is clear. Results for d 3 = 0.23 (with x 02 = 0.1025 and x& 02 = 9.369 ) in
the top plots show more extreme angular velocities and especially angular accelerations of the
crank. This is due to their approach to a singular configuration at d 3 = 0.22. These results are
similar to those of Fig. 5.10.10 of Section 5.10 for the same model without friction. The effects
of friction are seen in the significant reduction in angular velocity and phase differences in both
angular velocity and angular acceleration response.
666
Figure 8.10.7 omega1x and omega1xd vs t for Spatial Slider-Crank
As a result of the near singular configuration with d 3 = 0.23, constraint and friction
forces are large, leading to significant reduction in total energy. While increased numbers of
iterations were required in this near singular simulation, which led to 14 ´14 integration
Jacobians with condition numbers in the range 10 to 100 and an average step size of 5e-5 sec,
integration with the implicit trapezoidal method and Index 0 DAE formulation yielded reliable
results. With an error tolerance in numerical integration of e-6, maximum constraint error norms
over the simulation periods in position, velocity, and acceleration were 5e-11, 3.5e-8, and 2.5e-6,
respectively. Even in this near singular situation only 9 reparameterizations were required in
5011 time steps (557 time steps per reparameterization).
Both successful simulations reported above with the trapezoidal integrator fail when run
with the explicit integrators. This failure may occur due to the extreme nature of the dynamics, or
as a result of stiff equations of motion. Since the trapezoidal integrator is not well suited to treat
stiff systems, the system is likely not stiff, but a definitive judgment cannot be made.
8.10.3 Four Body Slider
A third example of friction and stiction simulation is the four mass spatial system shown
in Fig. 8.10.8 that was treated by ad-hoc derivation and coding in Section 8.3.2. As in the planar
three mass model problem, stiction will yield redundancy in this model. To see this, note that if
the velocity of mass one is zero, so will be the velocity of mass two, due to the distance
constraint between them. In turn, the velocity of mass 3 will be zero, due to the distance
constraint between it and mass two. This yields a third order redundancy for constraint reaction
forces during stiction.
667
Figure 8.10.8 Four Mass Spatial Model Problem
m1 = m2 = 2 kg, m3 = m4 = 6 kg , g = 9.8 m/sec2 ,

Problem data are l1 = 5 m, l2 = 7 m ,
k1 = k2 = k3 = 2 N/m , k4 = 10 N/m, md = 0.13, ms = 0.2 , and vt =10h . Initial conditions are
selected as q0 = [ 4 0 0 0 3 0 0 0 6.32 5] and
T
q& 0 = [ 0 0 0 0 0 0 0 0 0 -1] . Translational joint dimensions for all four joints are
T
a = b = 0.1 m. Integration tolerances in MATLAB Code 8.9 of Appendix 8.A are

u tol = B tol = e-8, in tol = e-6 , and Maxv = 0.7 . The AppData function for the model is as
shown in Fig. 8.10.9.
104 if app==3 %Four Body Slider

105
109
117 SJDT(:,1)=[5;1;0;z3;z3;0;uz;ux;uz;ux;0.1;0.1;0.14;0.12;1;5]; %Tran. - Body1 to Ground
118 SJDT(:,2)=[5;2;0;z3;z3;0;ux;uy;ux;uy;0.1;0.1;0.14;0.12;6;5]; %Tran. - Body2 to Ground
119 SJDT(:,3)=[5;3;0;z3;z3;0;ux;uz;ux;uz;0.1;0.1;0.14;0.12;11;5]; %Tran. - Body3 to Ground
120 SJDT(:,4)=[5;4;0;z3;z3;0;uz;ux;uz;ux;0.1;0.1;0.14;0.12;16;5]; %Tran. - Body3 to Ground
121 SJDT(:,5)=[1;1;2;z3;z3;5;z3;z3;z3;z3;0;0;0;0;21;1]; %Dist.-Bod1to2
122 SJDT(:,6)=[1;2;3;z3;z3;7;z3;z3;z3;z3;0;0;0;0;22;1]; %Dist.-Bod1to2
123
127 nv=ngc-nc;
128 nu=nc;
129
668
132 SMDT=[[2;1;1;1],[2;1;1;1],[6;1;1;1],[6;1;1;1]];
133
135 if NTSDA==0
137 end
141 STSDAT(:,1)=[1;0;z3;-10*ux;2;0;13;0];
142 STSDAT(:,2)=[2;0;z3;-10*uy;2;0;14;0];
143 STSDAT(:,3)=[3;0;z3;-10*uz;2;0;13;0];
144 STSDAT(:,4)=[1;4;z3;10*ux;10;0;11;0];
146 r10=[4;0;0];
147 p10=[1;0;0;0];
148 q10=[r10;p10];
149 r1d0=[0;0;0];
150 p1d0=[0;0;0;0];
151 q1d0=[r1d0;p1d0];
152 r20=[0;3;0];
153 p20=[1;0;0;0];
154 q20=[r20;p20];
155 r2d0=[0;0;0];
156 p2d0=[0;0;0;0];
157 q2d0=[r2d0;p2d0];
158 r30=[0;0;8.32];
159 p30=[1;0;0;0];
160 q30=[r30;p30];
161 r3d0=[0;0;0];
162 p3d0=[0;0;0;0];
163 q3d0=[r3d0;p3d0];
164 r40=[5;0;0];
165 p40=[1;0;0;0];
166 q40=[r40;p40];
167 r4d0=[-1;0;0];
168 p4d0=[0;0;0;0];
169 q4d0=[r4d0;p4d0];
170 q0=[q10;q20;q30;q40];
171 qd0=[q1d0;q2d0;q3d0;q4d0];
Figure 8.10.9 AppData Function, Four Body Slider
Results of a simulation using Code 8.9 and the implicit trapezoidal integrator with the
foregoing data are presented in Fig. 8.10.10, including a substantial period of stiction of masses
1, 2, and 3. Even though the geometry of joints and data in the present model and that of Section
8.3.2 are somewhat different, results presented in Figs. 8.3.2 and 8.10.10 are quite similar.
Reducing the coefficients of friction slightly led to the velocity plots of Fig. 8.10.11, with the
delay in initiation of stiction that has been seen earlier. A single parameterization sufficed for
each of these simulations, and the maximum norms of position, velocity, and acceleration
669
constraint errors over the simulation interval were 3.5e-12, 1.6e-10, and 2e-8, respectively.
Essentially identical results were obtained using the explicit Nystrom4 integrator, indicating that
the equations of motion are not stiff.
As a final note, it is interesting to compare the ease of use of Code 8.9 to the complexity
of ad-hoc derivation of equations of motion and coding in Section 8.3.2 and Code 8.3.2.
Figure 8.10.10 Positions and Velocities vs. Time, md = 0.12, ms = 0.14
Figure 8.10.11 Velocities vs. Time, md = 0.1, ms = 0.12
670
The disk (body 1) in Fig. 8.10.12 rotates in a revolute joint relative to ground (body 0)
about the global z axis. Body 2 translates in body 1 along a guide that is parallel to the body
fixed y1¢ axis, one unit to the right of the y1¢ axis. A TSDA with spring constant K = 10 N/m,
damping constant C = 0, and free length l 0 = 10.1 m acts between the origin of the x ¢2 -y¢2
reference frame and a point u1x¢ + 10u1y¢ in body 1. Static and dynamic coefficients of friction are
ms = 0.05 and ms = 0.04 , respectively and masses and moments of inertia of the bodies are
m1 = 10 kg, J aa = 10 kg × m 2 , m 2 = 5 kg, and J bb = 5 kg × m 2 . Dimensions of the joints are a = b =
R =0.1 m. Initial conditions for the system are q10 = [ 0 0 0] , q& 10 = [ 0 0 0] ,
T T
q 02 = [1 0 0] , and q& 02 = [ 0 0 0] . To simulate motion in a horizontal plane, the

T T
acceleration due to gravity is set to zero; i.e., g = 0.

The AppData function for simulation of this system using Code 8.9 is shown in Fig.
8.10.13. A simulation is carried out over a 10 sec interval, with plots of rotation theta1 of body 1
about the global z axis and translation of body 2 relative to body 1, y¢2 º dely2pr, shown in Fig.
8.10.14. Identical results were obtained with all four integration formulations, indicating that the
system is not stiff. Total energy is reduced by 96% over the simulation and the kinetic energy at
the final time was 7.3e-4 Nm. A simulation with damping coefficients ms = 0.08 and ms = 0.07
lead to the more rapid damping shown in Fig. 8.10.15, a reduction of 99.9 % in total energy over
the simulation and the kinetic energy at the final time was 7.3e-4 Nm. With larger coefficients of
friction, the integrators failed when total energy approached zero.

176
179
181
671
189 SJDT(:,1)=[4;1;0;z3;z3;0;ux;uz;ux;uz;0.1;0.1;0.08;0.07;1;5];%Rev - 1 to Ground
190 SJDT(:,2)=[5;1;2;ux;z3;0;ux;uy;ux;uy;0.1;0.1;0.08;0.07;6;5];%Tran. - 1 to 2
191
195 nv=ngc-nc;
196 nu=nc;
197
200 SMDT=[[10;10;10;10],[5;5;5;5]];
201
203 if NTSDA==0
205 end
209 STSDAT(:,1)=[1;2;ux+10*uy;z3;10;0;10.1;0];
210
212 r10=[0;0;0];
213 p10=[1;0;0;0];
214 r20=[1;0;0];
215 p20=[1;0;0;0];
216 q0=[r10;p10;r20;p20];
217 r1d0=[0;0;0];
218 p1d0=[0;0;0;0];
219 r2d0=[0;0;0];
220 p2d0=[0;0;0;0];
221 qd0=[r1d0;p1d0;r2d0;p2d0];
222 end
Figure 8.10.13 AppData Function, Rotating Disk with Translating Body
672
Figure 8.10.14 f1 = phi1 and dy¢2 = dely2pr, m s = 0.05 and md = 0.04
Figure 8.10.15 f1 = phi1 and dy¢2 = dely2pr, m s = 0.08 and md = 0.07
Four spatial examples simulated using the Index 0 DAE formulation and Code 8.9, show that
reliable and accurate results are obtained, including stiction that occurs in the first and third
examples. Even though the trapezoidal integration method is not well suited for treatment of
stiff systems, no difficulties were encountered in its use with each of the examples. This and
the fact that simulations in three of the examples using explicit integrators succeeded strongly
suggests that problems with friction are not necessarily stiff, contrary to what has been reported
in the literature for Index 3 DAE formulations.
673
Appendix 8.A Tangent Space Index 0 Friction Simulation Code
Code 8.1 Three Particle, Minimal Coordinates
Code 8.2 Three Particle, Piecewise Coulomb
Code 8.3.1 Three Particle, Cartesian Coord
Code 8.3.2 Four Particle, Cartesian Coord
Code 8.7 Planar Multibody Simulation with Friction
Code 8.9 Spatial Multibody Simulation with Friction
674
675
CHAPTER 9
Manipulator Kinematics and Dynamics
9.0 Introduction
Manipulators, also referred to as robots, differ from mechanical systems considered in
previous chapters in that they involve concepts of input, output, and control of motion.
Characteristics of manipulators are presented in Section 9.1 to illustrate means of exerting
control via inputs that may be electronically or hydraulically implemented. The distinction
between serial manipulators and parallel manipulators is noted, showing that parallel
manipulators involve a higher degree of analytical complexity and have increased capacity for
precision control of motion in the presence of extreme loads. Attention in the chapter is focused
almost exclusively on parallel manipulators.
Criteria for nonsingular manipulator forward and inverse kinematics are established in
Section 9.2, in terms of manipulator configurations that are characterized by input coordinates,
mechanism generalized coordinates, and output coordinates. Nonsingular behavior is shown to
be determined by the rank of four Jacobian matrices of functions in kinematic equations that
must uniquely determine input coordinates, generalized coordinates, and output coordinates in
the process of programming and controlling motion of a manipulator. Consequences of rank
deficiency in Jacobians of manipulator governing equations are shown to be the inability to
program and/or control the configuration and velocity of a manipulator. Criteria defined are local
in nature; i.e., they provide information only in a neighborhood of a given configuration.
Manifold theoretic methods are used in Section 9.3 to obtain global criteria for
nonsingular manipulator functionality. Domains of maximal, path connected, singularity free
manipulator functionality are defined as sets of manipulator configurations that satisfy four
matrix rank conditions at the configuration level. It is shown that boundaries of these
submanifolds in the manipulator configuration space are characterized by singularities that
preclude effective manipulator functionality. It is shown that any trajectory that connects
configurations in distinct domains of functionality must encounter a singularity. The focus of
methods developed is on definition of singularity free, path connected domains of functionality
that happen to have boundaries that are singular, rather on sets of singularities that are to be
avoided.
Four examples that illustrate use of analytical criteria to define singularity free domains
of functionality are presented in Sections 9.3 through 9.6, ranging from elementary illustrations,
to a planar front-end loader, to a spatial manipulator with 16 distinct domains of functionality.
A computational method for mapping boundaries of domains of functionality is
introduced in Section 9.7, followed by iterative methods for kinematic analysis at configuration
and velocity levels in Section 9.8.
Second order ODE of motion for manipulators are derived in Section 9.9, in terms of
input coordinates. They are shown to be well posed on each domain of functionality and become
singular as domain boundaries are approached. As illustrations, planar front-end loader and
spatial Stewart platform equations of motion are derived in Sections 9.10.
676
9.1 Characteristics of Manipulators
Examples that exhibit properties of manipulators are presented in this section, to provide
a basis for broadly applicable formulations and analysis methods.
9.1.1 Serial Manipulators
The conceptual robotic manipulator shown in Fig. 9.1.1 has seven actuated joints, with
relative coordinates that serve as inputs, denoted y1 = q i , i = 1,L,7, including a gripper between
bodies 8 and 9. A simplified three actuator model with body fixed coordinate systems and input
variables y1 , y2 , and y3 is shown in Fig. 9.1.2 and analyzed in Example 4.7.3. The three input
coordinates are to be chosen to control the x, y, and z coordinates of the outer end of body 3.
These manipulators enable explicit expressions for position and orientation of body i + 1, relative
to body i, as a function of yi . Such configurations are called serial manipulators in the literature.
Figure 9.1.1 Conceptual Serial Robotic Manipulator
Figure 9.1.2 Three Degree of Freedom Serial Manipulator
677
9.1.2 Spatial Stewart Platform Manipulator
The driving simulator motion control system shown in Fig. 9.1.3, called a Stewart
platform after its inventor, uses six hydraulic actuators to control the position and orientation of
the platform, inside which the vehicle cab and computer graphics interact with the driver. In
contrast with the serial nature of the manipulators shown in Figs. 9.1.1 and 9.1.2, there is not a
sequence of bodies and associated relative coordinates that control motion of the platform
relative to ground. This is, therefore, not a serial manipulator. Such a configuration, with
actuators that act in parallel, is called a parallel manipulator in the literature.
Figure 9.1.3 Stewart Platform in Driving Simulator Application

Among the reasons that parallel manipulators such as the Stewart platform are used in
applications, in spite of their analytical complexity, is their ability to maintain precision control
of the geometry of the system and to support extreme loads that occur during motion of massive
components, such as the driving simulator platform.
T
More specifically, a vector q = éër p ùû Î R of generalized coordinates defines the
T T 7
position and orientation of the platform, including Euler parameters p Î R 4 that define
orientation of the platform. The lengths y1 through y6 of the six actuators are input coordinates
that are shown schematically as dashed lines in Fig. 9.1.4, y Î R 6 . A scalar kinematic constraint
that enforces the Euler parameter normalization condition is
F(q) = pTp - 1 = 0 (9.1.1)
and six constraints that define actuator lengths as functions of generalized coordinates are of the
form
Y (q , y ) = 0 (9.1.2)
These relationships between inputs and platform generalized coordinates for a parallel
manipulator are much more complex than explicit kinematic expressions for output coordinates
as functions of input coordinates for serial manipulators.
678
Figure 9.1.4 Stewart Platform Geometry
9.1.3 Planar Stewart Platform Analog
A much simpler model of a manipulator with vector input and output coordinates is the
planar platform of Fig. 9.1.5 that is constrained so that its operating point O slides in a vertical
slot in the global frame. This yields two kinematic degrees of freedom and a vector
y = [ y1 y 2 ] of input coordinates yi ³ 0 that control the distance between points A1 and A 2 in
T
ground and B1 and B2 on the platform, with output coordinates z = [ z1 z 2 ] .

T
Figure 9.1.5 Two Degree of Freedom Planar Manipulator

Equations that relate input y and output z are
é ( (b1c - a 1 )2 + (z1 + b1s)2 - y12 ) / 2 ù
F(y , z ) = ê ú=0 (9.1.3)
ê( ( - b2c - a 2 )2 + (z1 - b2s)2 - y 22 ) / 2 ú
ë û
where s º sin (z2 ) and c º cos(z2 ) . This is an algebraic input-output model that does not require
introduction of generalized coordinates. The serial manipulators of Section 9.1.1 are also
represented by input-output models.
9.1.4 Tractor Front-End Loader
A related class of manipulator applications that involve heavy loads is material handling
equipment such as the front-end loader on the author’s tractor shown in Fig. 9.1.6. Such systems
consist of multiple moving components, whose configuration is controlled by hydraulic
actuators, as illustrated schematically in Fig. 9.1.7.
679
Figure 9.1.6 Tractor with Front-End Loader
Figure 9.1.7 Loader Geometry

As in the case of the Stewart platform of Fig. 9.1.3 and the planar analog of Fig. 9.1.5,
this manipulator has input hydraulic struts that act in parallel to control position and orientation
of the loader bucket, the output. This is thus not a serial manipulator.
The front-end loader shown in Fig. 9.1.8 is used for scooping material on the ground as
the chassis moves forward, tilting the bucket in an upward orientation to hold the material,
raising the bucket, and rotating the bucket downward to dump the material into a container or
vehicle. The forward and reverse motion of the chassis is suppressed in this example. The
mechanism has three bodies that move relative to body 0 (the chassis), whose positions and
orientations are defined in the chassis-fixed x-y frame by three generalized coordinates shown in
Fig. 9.1.8, q = [q1 q 2 q 3 ] . Hydraulic actuator inputs y = [ y1 y2 ] control motion of the
T T
T
system, and outputs are the orientation and elevation of the bucket, z = éë q1 + q 3 y D 2 ûù . Body 2
slides along the axis of body 1 (boom) and is connected to point D1 on body 3 (bucket) by a bar
of fixed length to control orientation of the bucket.
680
Parallel actuators act in this manipulator, so it is not a serial manipulator. It differs
fundamentally from the non-serial manipulators of Sections 9.1.2 through 9.1.4, in that there is a
kinematic closed loop involving the boom, slider, bucket, and bar that connects the bucket and
slider. It is a kinematically parallel manipulator, which is typical of manipulators found in most
heavy duty material handling, construction, and earth moving applications. Due to the kinematic
closed loop in this system, it cannot be represented by a continuous input-output model. The
focus of this chapter is on this class of manipulators.
9.1.6 Relation of Formulation Outlined for Parallel Manipulators to the Literature
In a survey of parallel manipulators, Merlet, Gosselin, and Huang (2016) identify
numerous open issues regarding manipulator analysis, among them the following:
(1) Formulations need to account for singularities that input-output models fail to identify.
(2) Singularity free components of manipulator workspaces need to be determined and
criteria established for movement from one component to another without encountering
singularities.
Relative to issue (1), Conconi and Carricato (2009) present an extensive analysis of
parallel manipulator singularities. They show that manipulator formulations require inclusion of
mechanism generalized coordinates and associated constraints, in order to account for
singularities that are inherent in the underlying mechanism and have nothing to do with input or
output coordinates. They expand classifications of singularities that are presented in the
literature, further reinforcing the need for inclusion of mechanism generalized coordinates and
associated constraints in parallel manipulator models that cannot be represented in input-output
form. While their approach extends the conventional input-output parallel manipulator
formulation, it is limited to manipulators that are comprised of a single platform whose motion is
controlled by parallel mechanisms that connect the platform to a fixed base. This is a
generalization of leg actuated systems, such as the Stewart platform of Section 9.1.2, but retains
the concept of a single platform whose motion is controlled by parallel input actuators.
A parallel manipulator kinematics formulation is presented in Section 9.2 that is based on
mechanism generalized coordinates and associated constraints, in addition to input and output
681
coordinates and their defining equations. Attention is restricted to nonredundant parallel
manipulators, for which the dimension of input and output coordinates is equal to the number of
degrees of freedom of the underlying mechanism, generally smaller than the number of
generalized coordinates. Four Jacobian (derivative) matrices are defined at the configuration
level that must be nonsingular for local existence and uniqueness of forward and inverse
kinematics. This formulation accounts for singularities that cannot be dealt with in input-output
form, either because the underlying mechanism has kinematic singularities or because
complexity of the mechanism precludes analytical reduction to input-output form. In either case,
the class of manipulators treated is much broader than parallel manipulators that have a single
moving platform and can be represented by an input-output model. It is emphasized that this
chapter is not another in a long list of treatments of parallel manipulator singularities; its purpose
is to define and characterize singularity free domains of functionality in which manipulators can
be reliably programmed and controlled.
It is shown in Section 9.3 that physically meaningful forward and inverse kinematic
mappings derived in Section 9.2 yield a mathematical structure for manipulator configuration
space that is in fact a differentiable manifold. Basic results of differential geometry are used to
show that the manifold is naturally partitioned into disjoint domains of functionality that are path
connected, maximal domains of singularity free manipulator operation. It is further shown that
any continuous manipulator configuration trajectory that begins in one domain of functionality
and proceeds to another must encounter a singularity. Implementation of the formulation uses
only linear algebra and multivariable calculus, without the need for abstract constructs of
differential topology. It is noted that these results are obtained with configuration criteria for
singularities, in contrast with velocity criteria used in most parallel manipulator literature.
A simple slider-crank parallel manipulator is used in Sections 9.2 and 9.3 to illustrate
concepts presented. The planar platform of Section 9.1.3 is treated in Section 9.4. The front-end
loader of Section 9.1.5 is treated in Section 9.5. A model problem with a seven-dimensional
configuration space and sixteen domains of functionality is treated in Section 9.6. yielding
explicit definition of singularity free domains and input-bounded domains that uniformly avoid
singularities.
A method for computation of boundaries of domains of functionality is presented in
Section 9.7. A broadly applicable computational approach for manipulator kinematic analysis is
presented in Section 9.8 and extended to an ODE formulation of manipulator dynamics in
Section 9.9 that requires no ad-hoc derivation of equations of motion and no DAE of motion.
Finally, ODE of dynamics are derived for the front end loader and a Stewart platform in Sections
9.10 and 9.11.
Parallel manipulators that are ideally suited for motion control under the influence of large forces are
contrasted with more classical serial manipulators that form the foundation of the field of robotics. The
focus of this chapter is on parallel manipulators and their parameterization, using the tangent space
formulation of Chapter 5. It is shown that manipulator input and output mappings yield a rich manifold
theoretic structure for kinematics of parallel manipulators. Finally, the approach of Chapter 5 is applied
to obtain ODE of motion in terms of manipulator input coordinates that are embedded in nonsingular
domains of functionality and require no ad-hoc derivation.
682
9.2 Parallel Manipulator Coordinates and Local Relationships
Most literature on parallel manipulators (Gosselin and Angeles, 1990; Merlet, 2006) uses
an input-output model such as Eq. (9.1.3) in which input coordinates y Î R k and output
coordinates z Î R k are related by k equations of the form
F( y , z ) = 0 (9.2.1)
where F : R k ´ R k ® R k is assumed to be continuously differentiable. The time derivative of Eq.
(9.2.1) yields velocity equations
Fy ( y, z )y& + Fz (y , z )z& = 0 (9.2.2)
Velocity-based singularity criteria are defined in this formulation as Fy ( y , z ) = 0 and/or

Fz (y , z ) = 0 , associated with the condition that a specified output and/or input velocity fails to
determine an input and/or output velocity.
The input-output model of Eqs. (9.2.1) and (9.2.2) is based on the assumption that
mechanism generalized coordinates that satisfy nonlinear kinematic constraints can be
analytically eliminated. For mechanisms used in industrial manipulators, this is generally not
possible.
9.2.1 Manipulator Coordinates and Kinematic Equations
As shown schematically in Fig. 9.2.1, the mechanical component of a parallel
manipulator is a mechanism, whose configuration is defined by generalized coordinates q Î R n
that are subject to m holonomic constraints,
Φ (q ) = 0 (9.2.3)
where Φ : R n ® R m is continuously differentiable. There are many choices for generalized
coordinates that specify the configuration of a mechanism, independent of any concept of input
or output associated with the mechanism, any of which may be employed in the present
formulation. As commonly used in the mechanical systems literature, such generalized
coordinates are not independent; i.e., they must satisfy holonomic constraints of Eq. (9.2.3).
Figure 9.2.1 Components of a Manipulator

A key requirement of generalized coordinates and the associated constraints of Eq.
(9.2.3) is that the constraint Jacobian Φ q (q ) has full rank, in which case the mechanism has
k = n - m degrees of freedom in a neighborhood of q; i.e., degree of freedom is a local property
of the mechanism. In such neighborhoods, the implicit function theorem of Section 2.2.5 shows
that a nonsingular configuration can be uniquely defined by k of the n generalized coordinates
q Î R n , or by another choice of k variables.
683
Input coordinates y Î R k of Fig. 9.2.1 are selected to control motion of the manipulator
in neighborhoods of configurations at which Φ q (q ) has full row rank; i.e., where the mechanism
has k degrees of freedom. Input coordinates are related to mechanism generalized coordinates
through input equations,
Ψ (q , y ) = 0 (9.2.4)
where Ψ : R n ´ R k ® R k is continuously differentiable. In case an input coordinate yi is a
generalized coordinate q j , Eq. (9.2.4) contains the condition q j - y i = 0 . Equations (9.2.3) and
(9.2.4) may be combined as n equations in n + k variables,
é Φ(q ) ù
Ω(q, y ) º ê ú=0 (9.2.5)
ë Ψ(q, y ) û
which is a necessary but not sufficient condition to determine q as a function of y.
Similarly, output coordinates z Î R k of Fig. 9.2.1 define manipulator working capability
in neighborhoods of configurations at which Φ q (q ) has full rank. Output coordinates are related
to mechanism generalized coordinates through output equations,
Γ (q , z ) = 0 (9.2.6)
where Γ : R n ´ R k ® R k is continuously differentiable. In case an output coordinate z i is a
generalized coordinate q j , Eq. (9.2.6) contains the condition q j - z i = 0 . Equations (9.2.3) and
(9.2.6) may be combined as n equations in n + k variables,
é Φ(q) ù
Λ(q, z ) º ê ú=0 (9.2.7)
ë Γ(q, z )û
which is a necessary but not sufficient condition to determine q as a function of z.
The manipulator configuration space is defined as the set of all manipulator coordinates
T
x = ëé y T
qT z T ûù Î R k ´ R n ´ R k = R n +2k that satisfy Eqs. (9.2.3), (9.2.4), and (9.2.6); i.e.,
X º {x Î R n +2k : Φ(q ) = 0, Ψ(q, y ) = 0, and Γ(q, z ) = 0} (9.2.8)
Endowed with open sets of the form V Ç X Ì X , where V is open in R n + 2 k , X is a topological

subspace of R n + 2 k (see Sections 2.2.4, 3.5, and 3.6). This is an important property that provides
structure for the equations of manipulator kinematics. It should be noted that the manipulator
configuration space of Eq. (9.2.8) is very different from the kinematic configuration space
C = {q Î R n : Φ(q ) = 0} of Section 3.5, called the c-space of the mechanism in the parallel
manipulator literature.
9.2.2 Slider-Crank Illustration
As a simple illustration of the foregoing formulation, consider the slider-crank
manipulator of Fig. 9.2.2, with a crank of unit radius and a connecting rod of length l ³ 1 units.
Mechanism generalized coordinates for the one degree of freedom slider-crank mechanism are
684
the angle of rotation q1 of the crank relative to the x axis and the x coordinate q 2 of the slider,
q = [q1 q 2 ] Î R 2 . An actuator strut is the input coordinate of length y Î R1 and the output
T
coordinate is z = q 2 Î R1 . Manipulator coordinates are thus x = [ y q1 q 2 z] Î R 4 .

T
Figure 9.2.2 Slider-Crank Manipulator

Constraint, input, and output equations of Eqs. (9.2.3), (9.2.4), and (9.2.6) for this
manipulator are
Φ(q ) = ( (q 2 - c)2 + s2 - l2 ) / 2 = ( q 22 - 2q 2c + 1 - l2 ) / 2 = 0
Ψ(q,y) = ( (c + 2)2 + s2 - y 2 ) / 2 = ( 4c + 5 - y 2 ) / 2 = 0 (9.2.9)
Γ(q,z) = q 2 - z = 0
where c º cosq1 and s º sin q1 . At q = [0 1 ± l ] , F q1 = 0 and F q2 = ±2l ¹ 0 . The symmetric

T
implicit function theorem (Dontchev and Rockafellar, 2014) shows that q1 cannot be written as a
differentiable function of q 2 = z in neighborhoods of q . Thus, this manipulator model cannot be
differentiably transformed to input-output form in any neighborhood of q . The same is true in
any neighborhood of q = [ π -1 ± l ] .
T
9.2.3 Forward and Inverse Kinematics

T
At a manipulator configuration x = éë y T qT z T ùû Î X , conditions are required that
assure Eqs. (9.2.5) and (9.2.6) uniquely determine q and z as differentiable functions of input y
in a neighborhood of x ; i.e., nonsingular forward kinematics. Conversely, conditions are
required that assure Eqs. (9.2.4) and (9.2.7) uniquely determine q and y as differentiable
functions of output z in a neighborhood of x ; i.e., nonsingular inverse kinematics. Such
conditions, in terms of the functions defined in Eqs. (9.2.4) through (9.2.7), are provided by the
implicit function theorem of Section 2.2.5 as
Ψ y (q, y ) ¹ 0 (9.2.10)
Γ z (q , z ) ¹ 0 (9.2.11)
é Φq (q ) ù
Ωq (q, y ) = ê ú ¹0 (9.2.12)
ë Ψ q (q , y ) û
é Φ q (q ) ù
Λ q (q , z ) = ê ú ¹0 (9.2.13)
ë Γ q (q, z ) û
685
To account for singular points noted in Section 3.7 at which Φ q (q ) fails to have full row rank, it
has been suggested that an additional criterion is needed, namely, rank ( Φq (q ) ) = n - k ; i.e.,
Φ q (q ) has full rank. Since full rank of Φ q (q ) is implied by Eqs. (9.2.12) and (9.2.13), it need
not be redundantly imposed.
It is important to note that equality in any of the four conditions of Eqs. (9.2.10) through
(9.2.13) defines a singular configuration, whereas singularities at the velocity level are defined
with the input-output model of Eq. (9.2.1) based on only two conditions, Fy (y, z) = 0 and/or
Fz (y, z) = 0 . In particular, the present formulation accounts for singularities due to rank
deficiency of Φ q (q ) that appears in Eqs. (9.2.12) and (9.2.13), as well as other forms of
singularity. The input-output model cannot account for such singularities.
The manipulator configuration space X of Eq. (9.2.8) may include singular configurations
in which forward and/or inverse kinematics fail, so it is too large a space for reliable manipulator
operation. The four conditions of Eqs. (9.2.10) through (9.2.13) will be shown to eliminate such
negative forms of behavior. This suggests defining the regular manipulator configuration space,
ìï Ωq (q, y ) ¹ 0, Ψy (q, y ) ¹ 0, üï
%
X = íx = éëy T q T T T
z ùû Î X : ý (9.2.14)
ïî Λ q (q, z ) ¹ 0, and Γ z (q, z ) ¹ 0 ïþ
In connection with issue (2) of Section 9.1.6, Chablat and Wenger (1998) define aspects
as sets of manipulator configurations such that Fy ( y , z ) ¹ 0 and/or Fz ( y, z ) ¹ 0 , using matrices
in the input-output formulation of Eq. (9.2.2). This is related to the regular manipulator
configuration space of Eq. (9.2.14), but is limited to the input-output formulation and lacks the
ability to account for singularities that are functions of manipulator generalized coordinates and
associated constraints.
The implicit function theorem of Section 2.2.5 provides a sufficient condition that an
input y uniquely determines the configuration q of a manipulator in a neighborhood of (q , y )
that satisfies Eq. (9.2.5), namely the condition of Eq. (9.2.12); i.e., there is a unique continuously
differentiable solution of Eq. (9.2.5) in a neighborhood Y of y ,
q = f (y ) (9.2.15)
A sufficient condition that q determines a unique output z that satisfies Eq. (9.2.6) in a
neighborhood of ( q , z ) = ( f ( y ), z ) is Eq. (9.2.11); i.e., there is a unique continuously
differentiable solution of Eq. (9.2.6) in a neighborhood W of q ,
z = h(q ) (9.2.16)
If the conditions of Eqs. (9.2.11) and (9.2.12) hold, then Eqs. (9.2.15) and (9.2.16) uniquely
determine an output for an input in a neighborhood of ( y , q, z ) ; i.e., forward kinematics succeed.
This can fail for two reasons. First, even if Φ q (q ) has full rank, Eq. (9.2.12) may fail to hold, in
which case Eq. (9.2.5) fails to determine a unique q. Second, if the condition of Eq. (9.2.11) fails
686
to hold, then Eq. (9.2.6) fails to determine a unique output z for an admissible configuration q of
the mechanism. In either case, forward kinematics fail.
In order for an output z to uniquely determine the configuration q of a manipulator in a
neighborhood of ( q, z ) that satisfies Eq. (9.2.7), Eq. (9.2.13) implies that there is a unique
continuously differentiable solution of Eq. (9.2.7) in a neighborhood Z of z ,
q = e(z ) (9.2.17)
Finally, Eq. (9.2.10) implies that there exists a unique continuously differentiable solution of Eq.
(9.2.4) in a neighborhood W of q , where ( q, y ) satisfies Eq. (9.2.4),
y = g(q ) (9.2.18)
If the conditions of Eqs. (9.2.10) and (9.2.13) hold, then Eqs. (9.2.17) and (9.2.18)
uniquely determine an input to achieve the desired output in a neighborhood of ( y , q, z ) ; i.e.,
inverse kinematics succeed. This can fail for two reasons. First, even if Φ q (q ) has full rank, Eq.
(9.2.13) may fail to hold, in which case Eq. (9.2.7) fails to determine a unique q. Second, if the
condition of Eq. (9.2.10) fails to hold, Eq. (9.2.4) fails to determine a unique input y that leads to
a kinematically admissible configuration of the mechanism. In either case, inverse kinematics
fail.
If the conditions of Eqs. (9.2.10) through (9.2.13) hold at a configuration x Î X , then in
neighborhoods of x the functions f, g, h, and e of Eqs. (9.2.15), (9.2.16), (9.2.17), and (9.2.18)
exist and are continuously differentiable. With input y in a neighborhood of y , Eqs. (9.2.15) and
(9.2.16) yield a unique manipulator output,
z = h ( f ( y )) (9.2.19)
This is the forward kinematic mapping, which is essential in control of the manipulator; i.e.,
determining a locally unique output due to a given input in neighborhoods of y and z .
Conversely, with output z in a neighborhood of z , Eqs. (9.2.17) and (9.2.18) yield
y = g(e(z )) (9.2.20)
which is a unique input that yields the desired output in neighborhoods of z and y . This is the
inverse kinematic mapping, which is essential in programming the manipulator; i.e., determining
a locally unique input that yields the desired output.
If any of the functions e( z ), f ( y ), g(q ), or h(q ) fail to exist in a neighborhood of x Î X ,
the forward and/or inverse kinematic mapping fails and x is called a singular manipulator
configuration. This definition, which is based on mappings at the configuration level, is not
commonly used in the literature on parallel manipulator singularities, represented by
bibliographies in (Merlet, 2006; Conconi and Carricato, 2009; Merlet, Gosselin, and Huang,
2016 ) in which the definition of singularities is based on velocity conditions associated with
coefficient matrices in input-output velocity equations of Eq. (9.2.2). Input-output velocity
equations cannot be defined if either f ( y ) or e( z ) fails to exist, invalidating input-output velocity
singularity criteria in such cases.
687
Defining manipulator kinematics in terms of input, generalized, and output coordinates enables
definition of criteria for existence of forward and inverse kinematics that determine the functionality of
the manipulator. With these criteria, the existence of locally unique mappings from input to output
coordinates, through generalized coordinates, is established, which is essential for controllability of the
manipulator. Conversely, the existence of locally unique mappings from output to input coordinates,
again through generalized coordinates, is established, which is essential for programmability of the
manipulator.
Key Formulas
é Φ(q ) ù
Φ (q ) = 0 Ψ (q , y ) = 0 Ω(q, y ) º ê ú=0 (9.2.3) (9.2.4) (9.2.5)
ë Ψ(q, y ) û
é Φ(q) ù
Γ (q , z ) = 0 Λ(q, z ) º ê ú=0 (9.2.6) (9.2.7)
ë Γ(q, z )û
X º {x Î R n +2k : Φ(q ) = 0, Ψ(q, y ) = 0, and Γ(q, z ) = 0} (9.2.8)
ì Ωq (q, y ) ¹ 0, Ψy (q, y ) ¹ 0, üï
% = ïíx = éy T
X qT
T
z T ùû Î X : ý (9.2.14)
ë
îï Λ q (q, z ) ¹ 0, and Γ z (q, z ) ¹ 0 þï
688
9.3 Singularity Free Domains of Functionality
Manipulator functionality relations presented in Section 9.2 are local in nature; i.e., they
are limited to neighborhoods of configurations in which criteria of Eqs. (9.2.10) through (9.2.13)
hold. Differential geometry is next employed to extend these relations to the full range of
manipulator operation; i.e., to maximal domains of singularity free functionality.
9.3.1 Differentiable Manifolds of Manipulator Functionality
Theorem 9.3.1: The regular manipulator configuration space X % of Eq. (9.2.14),
parameterized by the kinematic relations of Section 9.2.3, is a differentiable
manifold.
To prove the theorem, for any x i Î X % in a sequence of points in X% there is a
neighborhood Wi = Y i ´ Qi ´ Zi Ì X of x i in which the continuously differentiable functions
f i : R k ® R n and h i : R n ® R k defined in Eqs. (9.2.15) and (9.2.16) yield the mapping
ψi : Y i ® X %,
ψi ( y ) = ëé y T
T
%
f i ( y )T h i ( f i ( y ))T ûù = x Î X (9.3.1)
which is differentiable, since functions f and h are differentiable. Define the mapping
% ® R k in W i as
fi : X
f i (x ) = y (9.3.2)
T
Since ψi (f i (x )) = éë y T f i ( y )T h i (f i ( y ))T ùû = x for all x Î W i , fi = ψ i( -1) . The
homeomorphisms ψi and fi on W i , called charts in Sections 3.5 and 3.6, for a countable
¥
% ; i.e., X% = W i , define X
% as a manifold with local
number of neighborhoods W i that cover X U i =1
coordinates y (see Section 3.5).
To complete the proof that the manifold is differentiable, it must be shown that
% with neighborhoods
intersecting charts are compatible. For any pair of points x i and x j in X
T
W i and W j that overlap; i.e., W i Ç W j is not empty, for x = éë y T qT z T ùû Î Wi Ç W j ,
f i ( y ) = f j ( y ) and hi ( y ) = h j ( y ) . From Eqs. (9.3.1) and (9.3.2), y i ( y ) = y j ( y ) and fi (x ) = f j (x ) .
Thus, the charts (fi ,Wi ) and ( f j ,W j ) are compatible and define a smooth structure
(Schlichtkrull, 2015; Dundas, 2018), so the manifold is differentiable. This completes the proof.
Differentiability of the manifold yields key theoretical results in the present development
and is essential in construction and solution of differential equations of motion on the manifold
that are needed in implementation of algorithms for control of manipulator dynamics (Merlet,
2006). These results do not follow from velocity criteria for singularities in the input-output
formulation.
For x i Î W i , the continuously differentiable functions ei : R k ® R n and gi : R n ® R k
defined in Eqs. (9.2.17) and (9.2.18) yield the mappings
689
ψi ( z ) = éëg i (ei ( z ))T e i ( z )T
T
% and f i (x ) = z , with
z T ùû = x Î X
ψi ( f i (x )) = éëg i (e i ( z))T e i ( z )T
T
% is a
z T ùû = x for all x Î W i . This again shows that X
manifold, but with z serving as local coordinates. With requirements for both forward and
inverse kinematics embedded in the conditions of Eq. (9.2.14), there is a duality between input
and output coordinates.
It is interesting to note that existence of manipulator forward and inverse kinematics
dictate that X % is a differentiable manifold. An abstract mathematical development was not
required to obtain this result. In using manifold theory, it is important to note that a set of
configurations is a manifold only if charts ( y i , fi , W i ) are defined; i.e., sets without charts are
not necessarily manifolds. For more on the basics of manifolds, see Section 3.6.
9.3.2 Singularity Free Domains of Functionality
A consequence of the fact that X% is a differentiable manifold is that it is an open subset
of X in the topology induced by R n +2k , which can be partitioned into maximal (in the sense of
inclusion) connected components X % such that X % ÇX % is empty, for all i ¹ j , and X % = ÈX%
i i j i
i
(see Section 3.6). Since X %

% is a differentiable manifold, it is shown in Section 3.6 that the sets X
i
are path connected; i.e., any pair of configurations x and x in X

1 2 % is connected by a continuous
i
mapping ω : [0,1] ® X % , with ω(0) = x1 and ω(1) = x 2 . It should be noted, however, that the X
%
i i
may not be simply connected (Mendelson, 1962). A result that follows from manifold theory is
that the manipulator has k degrees of freedom throughout each X % ; i.e., the local degree of
i
freedom concept is made global on domains of functionality X . %

i
Since the subsets X% of X % are path connected, with paths defined by the forward and
i
inverse kinematic relations of Section 9.2, the conditions of Eq. (9.2.14) must hold throughout
each X% ; i.e.,
i
ì Ω q (q , y ) = d i1 ( x ) ¹ 0, Ψ y (q, y ) = d i2 ( x ) ¹ 0, ü
ïï ïï
% = íx Î X : Λ (q, z ) = d ( x ) ¹ 0, and Γ (q, z ) = d ( x ) ¹ 0,
X ý (9.3.3)
i q i3 z i4
ï ï
ïî with sign(d ij ( x )) constant throughout ïþ
% are open in X, so they are manifolds; i.e., submanifolds of X
The sets X %.
i
% are k-dimensional manifolds that are subsets of R n +2k . While if

It is important that X i
% onto the input space Y Ì R k or the output space
k = 1, 2, or 3, drawings of projections of X i
Z Ì R may be possible, for k > 3, visualization of X

k % Ì R n +2k is difficult. The control system
i
engineer must deal with relations in R n + 2k

that define X% , a daunting task. While pictures in
i
% onto Y or Z may be helpful in some cases, they are not a substitute for dealing
projections of X i
with the manifold X% Ì R n +2k .

i
690
On boundaries of the X % in X, at least one of the determinants of Eq. (9.3.3) must be
i
%
zero. As a result, X is the union of disjoint submanifolds X % , i = 1,2,L , each of which is a
i
maximal singularity free open subset of X in which signs of the four determinants of Eq. (9.2.14)
are invariant.
The submanifolds X % , called domains of functionality, are maximal, path connected,
i
open sets of singularity free configurations in X. Each X% is a manifold with the

i
parameterization of Eq. (9.3.1) on open subsets of X % , called charts in manifold theory (see
i
Section 3.6). The extension of local forward and inverse kinematic relations derived in Section
9.2 to this global setting is a result that differential geometry brings to parallel manipulator
theory. While these results are consequences of differential geometry, details of the extensive
theory of differential geometry are not required for their use. Once established, implementation
of these results requires only use of linear algebra and multivariable calculus.
The preceding results may be summarized as:
Theorem 9.3.2: The singularity free domains of functionality of Eq. (9.3.3) are
disjoint, maximal, path connected, submanifolds of regular manifold
configuration space. Any trajectory between distinct domains must encounter a
singularity.
% in X Ì R n +2k is
The boundary of a domain of functionality X i
ì for arbitrarily small δ, there is a w Î X % with ü

ïï i
ïï
%
¶X i = í Î X :
x w Î B( ,δ) such that q ( , ) = 0,
x Ω q y ý (9.3.4)
ï ï
ïî Ψ y (q , y ) = 0, Λ q (q , z ) = 0, or Γ z (q, z ) = 0 ïþ
where B(x ,δ) = {v Î X: v - x < δ} is the open ball of radius δ centered at x (see Section 2.2.4).
% are disjoint, a continuous transition of manipulator configuration from one such
Since the X i
domain to another requires crossing the boundaries of each in X. That is, a continuous one-
% and x (1) Î X
dimensional trajectory x (t), 0 £ t £ 1 , in X with x (0) Î X % , i ¹ j , must pass
i j
% %
through ¶X and ¶X . On these boundaries, one or more of the conditions of Eq. (9.2.14) fails to
i j
hold and functionality of the manipulator is lost; i.e., the manipulator is in a singular
configuration. Thus, the configuration of a manipulator cannot make a continuous transition from
one domain of functionality to another, without encountering a singularity.
Connectivity of the closure of domains of functionality is illustrated schematically in Fig.
9.3.1. The closures in X of some domains X % ; e.g., X
% , may not intersect closures of others;
i 1
% and X
they may intersect at a single boundary point; e.g., X % ; they may intersect along a
2 3
% and X
common boundary segment; e.g., X % ; or they may intersect along numerous boundary
4 5
% through X
segments; e.g., X % . As will be seen in applications, the latter case often occurs. In
6 9
691
any case, a continuous transition from one domain of functionality to another cannot occur
without encountering a singularity.
Figure 9.3.1 Closures of Domains of Functionality

If closures of a pair of domains intersect, external influence may be exerted to make a
continuous transition across the common singular boundary, so the mechanism does not need to
be disassembled and reassembled to make the transition, but it does encounter a singularity. In
contrast, if the closures of two domains do not intersect, the mechanism must be disassembled
and reassembled to make a transition between the domains. Examples of domains that illustrate
behaviors shown in Fig. 9.3.1 are presented for the slider-crank example in this section, a planar
platform in Section 9.4, a front-end loader in Section 9.5, and a model problem in Section 9.6.
A substantial literature has emerged, e.g., (Innocenti and Parenti-Castelli, 1998; Peidro,
Marin, Gil, and Reinoso, 2015), demonstrating that transitions between some configurations,
called assembly modes, can occur without encountering a singular configuration. On the surface,
this appears to be inconsistent with the foregoing results. A review of this literature, with the
help of A. Peidro, reveals that the “assembly modes” referred to are in fact multiple
T
configurations x i = éë y T qiT z iT ùû , i = 1,2, x1 ¹ x 2 , both of which are in X of Eq. (9.2.8) with
the same input y. It is shown in the cited literature, using examples, that for some manipulators a
pair of “assembly modes” can be connected by a trajectory x (t) that does not encounter a
singularity. The foregoing theory shows that such pairs of configurations must be in the same
domain of functionality. If domains of functionality are determined, one need only check to see
if a pair of “assembly modes” is in the same domain to know whether they can be connected by a
nonsingular path. This behavior is illustrated using the front-end loader in section 9.5.
9.3.3 Domains of Functionality with Input Bounds
All domain boundaries in Fig. 9.3.1 represent singular configurations in which
unacceptable behavior such as bifurcation or infinite constraint forces may occur. Since these
configurations must be strictly avoided in control of a manipulator, limits yi min < yi < yi max are
often imposed on some or all inputs. Such bounds may be hard, in the sense that they are
physical limits on actuator travel, or they may be soft, in the sense that they are nominal limits
and additional cushion exists in the hardware implementation to accommodate uncertainty. This
is common practice in virtually all industrial manipulator designs. Input bounds may be enforced
by transformation to new input coordinates v i ,
y i (v i ) = (1/2) ( (yimin + y imax ) + (y imax - yimin )sin v i ) , - π / 2 < v i < π / 2 (9.3.5)
692
If bounds are placed on all inputs; i.e., y = y (v ) , the input equation of Eq. (9.2.4) becomes
Ψ(q, v ) = Ψ (q, y (v )) = 0 (9.3.6)
and, using the chain rule of differentiation, its Jacobian with respect to v is
Ψ v (q, v ) = Ψ y (q, y )y v = Ψ y (q, y )diag ( 0.5 ( y imax - y imin ) cos v i ) (9.3.7)
The determinant of a product is the product of its determinants, so
π ( 0.5 ( y imax - y imin ) cos v i )

k
Ψ v (q, v ) = Ψ y (q, y ) (9.3.8)
i =1
Since 0.5 ( y imax - y imin ) ¹ 0 and cos vi ³ 0 in the range -π / 2 < vi < π / 2 of Eq. (9.3.5), the
domains of functionality of Eq. (9.3.3) become
ì Ωq (q, y (v )) = d i1 ( x ) ¹ 0, Ψ y (q, y (v )) cos v s ¹ 0, s = 1,L , k, ü
ïï ïï
% ¢ º í x Î X¢ : Λ (q, z ) = d ( x ) ¹ 0, and Γ (q, z ) = d ( x ) ¹ 0,
X ý (9.3.9)
i q i2 z i3
ï ï
with sign(d ij ( x )) constant throughout
îï þï
{
X ¢ = x = éë v T qT
T
z T ùû : Φ(q ) = 0, Ψ(q, y(v )) = 0, and Γ(q, z ) = 0 } (9.3.10)
From a set theoretic point of view, if the set of admissible inputs is decreased, due to
adding the constraints of Eq. (9.3.5), the set of feasible configurations of the manipulator is
decreased, hence the domains of functionality are decreased; i.e., X %¢ÌX % Ì X . This is
i i
illustrated graphically in Fig. 9.3.2, as it relates to Fig. 9.3.1. In fact, if the bounds of Eq. (9.3.5)
are increased in severity; i.e., yimin is increased and/or yimax is decreased, some X % ¢ may
i
disappear; i.e., become empty. Effects of input bounds on domains of functionality are illustrated
with the front-end loader of Section 9.5.
Figure 9.3.2 Closures of Input Bounded Domains of Functionality

9.3.4 Domains of Functionality for the Slider-Crank Manipulator
For the slider-crank example of Section 9.2.2, Ψ y (q, y) = -y ¹ 0 , Γz (q,z) = -1 ¹ 0 ,
Φq (q ) = [sq 2 (q 2 - c)] , Ψ q (q, y) = [ -2s 0] , and Γq (q, z) = [0 1] . The conditions of Eqs.
(9.2.10) and (9.2.11) are thus satisfied and those of Eqs. (9.2.12) and (9.2.13) are
693
é sq q 2 - c ù
Ωq (q, y) = ê 2 = 2s(q 2 - c) ¹ 0
ë -2s 0 úû
(9.3.11)
ésq q2 - cù
Λ q (q,z) = ê 2 = sq 2 ¹ 0
ë 0 1 úû
Based on the conditions of Eq. (9.3.11), the domains of functionality of Eq. (9.3.3) are
% = {x Î X : s ¹ 0, q - c ¹ 0, and q ¹ 0 throughout}
X (9.3.12)
i 2 2
9.3.4.1 Domains of Functionality with l > 1
From Eq. (9.2.9), q 2 = c ± c 2 + (l2 - 1) , y = 5 + 4c , z = q 2 , and

q 2 - c = ± c 2 + (l 2 - 1) ¹ 0 , so Eq. (9.3.12) reduces to
% = {x Î X : s ¹ 0 and q ¹ 0, throughout}
X (9.3.13)
i 2
where, parameterized by q1 , the slider-crank manipulator configuration space is
ì T
ü
X = íx = [ y, q1 , q 2 , z ] = é 5 + 4c, q1 , c ± c 2 + (l2 - 1), c ± c 2 + (l2 - 1) ù ý
T
(9.3.14)
î ë û þ
All domains of functionality that satisfy Eq. (9.3.13) are as follows
X 1 {
% = x Î X: s > 0 and q = c + c 2 + (l2 - 1) > 0
2 }
ì T
ü
= íé 5 + 4c q1 c + c 2 + (l2 - 1) c + c 2 + (l2 - 1) ù ; 0 < q1 < πý
îë û þ
X 2 {
% = x Î X: s > 0 and q = c - c 2 + (l2 - 1) < 0
2 }
ì T
ü
= íé 5 + 4c q1 c - c 2 + (l2 - 1) c - c 2 + (l2 - 1) ù ; 0 < q1 < πý
îë û þ
(9.3.15)
X 3 {
% = x Î X: s < 0 and q = c + c 2 + (l2 - 1) > 0
2 }
ì T
ü
= íé 5 + 4c q1 c + c 2 + (l2 - 1) c + c 2 + (l2 - 1) ù ; - π < q1 < 0ý
îë û þ
X 4 {
% = x Î X: s < 0 and q = c - c 2 + (l2 - 1) < 0
2 }
ì T
ü
= íé 5 + 4c q1 c - c 2 + (l2 - 1) c - c 2 + (l2 - 1) ù ; - π < q1 < 0ý
îë û þ
Each of these nonempty domains is an open, one-dimensional set (curve) in manipulator
configuration space X Ì R 4 , parameterized by q1 . Each domain is singularity free; i.e., the
manipulator can be reliably programmed and controlled in each domain, with no singularities in
its interior. The only singularities are at endpoints, which are not in the domains.
694
Since z = q 2 , the four domains of functionality with l = 2 are shown as plots in the
three-dimensional q -q -y frame of Fig. 9.3.3, denoted Xi = X % , i = 1,L,4 . This behavior is an
1 2 i
instance of two pairs of domains (X % ,X% ) and (X % ,X% ) , shown as X % and X % , i = 1,2, in Fig.
1 3 2 4 2i 3i
9.3.1, the closures of which intersect at isolated points in each pair, but there is no intersection of
the closures of the two pairs. Thus, to make a transition from one pair to the other, the
mechanism must be disassembled and reassembled.
Figure 9.3.3 Domains of Functionality of Slider-Crank for l = 2

To determine whether continuous transitions are possible between X % and X % or
1 3
% %
X 2 and X 4 , midpoints of the ranges of q1 that define these domains in Eq. (9.3.15) are used to
evaluate the determinants of Eq. (9.3.11), with results presented in Table 9.3.1. Since signs of
both determinants differ in these pairs of domains, they cannot be connected by a singularity free
trajectory, consistent with the theory. The converse of this test is inconclusive; e.g., the signs of
determinants are equal for X % and X% and for X % and X % , yet these domains are distinct.
1 4 2 3
Table 9.3.1 Determinants Ωq (q, y ) and Λ q (q, y )
%
X Point q1 q2 Ωq (q, y ) Λ q (q, y )
i
%
X (1) π/2 3 2 3 3
1
%
X (2) π/2 - 3 -2 3 - 3
2
%
X (3) -π / 2 3 -2 3 - 3
3
%
X (4) -π / 2 - 3 2 3 3
4
695
9.3.4.2 Domains of Functionality with l = 1
For l = 1 , the first of Eqs. (9.2.9) is q 2 ( q 2 - 2c ) = 0 , so if q 2 ¹ 0 , q 2 = 2c and q 2 - c = c .
Thus, the inequalities of Eq. (9.3.12) reduce to s ¹ 0 and c ¹ 0 , yielding
% = {x Î X : s > 0 and c > 0}
X 1
{
= éë 5 + 4c
T
q1 2c 2c ùû ; 0 < q1 < π / 2 }
% = {x Î X : s > 0 and c < 0}
X 2
{
= éë 5 + 4c
T
q1 2c 2c ùû ; π/2 < q1 < π } (9.3.16)
% = {x Î X : s < 0 and c > 0}
X 3
{
= éë 5 + 4c
T
q1 2c 2c ùû ; - π/2 < q1 < 0 }
% = {x Î X : s < 0 and c < 0}
X 4
{
= éë 5 + 4c
T
q1 2c 2c ùû ; - π < q1 < -π/2 }
Denoting Xi = X % , i = 1L 4 , these domains are plotted in the three-dimensional q -q -y frame of
i 1 2
% % % % % % %
Fig. 9.3.4. Closures of domains X and X , X and X , X and X , and X and X have points%
1 2 1 3 3 4 2 4
in common, in the sense that X % and X % have extensions beyond π . Table 9.3.2 presents data for
2 4
midpoints of each domain in Eq. (9.3.16) and signs of the determinants of Eq. (9.3.11). The signs
of both determinants differ in X% and X % , X
% and X% , X
% and X % , and X % and X% , so no
1 2 1 3 3 4 2 4
nonsingular trajectories can connect these domains, consistent with the theory.
Figure 9.3.4 Domains of Functionality of Slider-Crank for l = 1
696
%
X Point q1 q2 Ωq (q, y ) Λ q (q, y )
i
%
X (1) π/4 1.4 1 1
1
%
X (2) 3π / 4 -1.4 -1 -1
2
%
X (3) -π / 4 1.4 -1 -1
3
%
X (4) -3π / 4 -1.4 1 1
4
It is interesting to note that, as a function of l , this simple manipulator has not-so-simple

distinctly different domains of functionality. For any l > 1 , no matter how close to 1, the distinct
pairs of domains shown in Fig. 9.3.3 occur. The transition to the sequence of domains in Fig.
9.3.4 is thus a discontinuous function of l .
Existence of unique local forward and inverse kinematic mappings established in Section 9.2 is used to
prove that the regular manipulator configuration space is a differentiable manifold. Basic results of
differential geometry show that these local properties are in fact global on maximal, singularity free,
path connected domains of functionality, where the manipulator is controllable. Further, it shows that
any trajectory in manipulator configuration space between distinct domains of functionality must
encounter a singularity. While the theory that extends these local kinematic properties to global results
on domains of functionality is a bit deep, the result is concrete and its implementation requires use of
only linear algebra and multivariable calculus. This is an important result for manipulator kinematics
and dynamics that flows directly from a result of abstract mathematics.
Key Formulas
ì Ω q (q , y ) = d i1 ( x ) ¹ 0, Ψ y (q, y ) = d i2 ( x ) ¹ 0, ü
ïï ïï
% = íx Î X : Λ (q, z ) = d ( x ) ¹ 0, and Γ (q, z ) = d ( x ) ¹ 0,
X ý (9.3.3)
i q i3 z i4
ï ï
ïî with sign(d ij ( x )) constant throughout ïþ
ì for arbitrarily small δ, there is a w Î X % with ü

ïï i
ïï
% = í x Î X : w Î B(x ,δ) such that Ω (q, y ) = 0,
¶X ý (9.3.4)
i q
ï ï
îï Ψ y (q , y ) = 0, Λ q (q , z ) = 0, or Γ z (q, z ) = 0 þï
y i (v i ) = (1/2) ( (yimin + y imax ) + (y imax - yimin )sin v i ) , - π / 2 < v i < π / 2 (9.3.5)
697
9.4 Domains of Functionality for a Planar Manipulator
As an example of a manipulator with vector input and output, the planar platform of Fig.
9.4.1 has been studied by Peidro, Marin, Gil, and Reinoso (2015), using the input-output
formulation of Eq. (9.1.3). To illustrate use of the formulation of Sections 9.2 and 9.3, the
platform is constrained so that its operating point O slides in a vertical slot in the global frame.
This yields the vector of two generalized coordinates q = [q1 q 2 ] , a vector y = [ y1 y 2 ] of
T T
two input coordinates with y i ³ 0 that control the distance between points A1 and A 2 in ground
and B1 and B2 on the platform, and output coordinates z = q . Thus,
T
x = ëé y T qT z T ûù Î X Ì R 6 . Data for this example are a1 = 5, a 2 = -6, b1 = 6, and b2 = 5 in
dimensionless units, slightly modified from (Peidro et. al., 2015) to avoid algebraic complexities.
Figure 9.4.1 Two Degree of Freedom Planar Manipulator

There are no kinematic constraints on q and input equations that determine y as a
function of q are
é ( (b1c - a1 )2 + (q1 + b1s)2 - y12 ) / 2 ù
W (q, y ) = ê ú
ê( ( - b2c - a 2 )2 + (q1 - b2s)2 - y 22 ) / 2 ú
ë û
(9.4.1)
é( q12 + 12sq1 - 60c + 61 - y12 ) / 2 ù
=ê ú=0
ê( q12 - 10sq1 - 60c + 61 - y22 ) / 2 ú
ë û
where s º sin (q 2 ) and c º cos(q 2 ) . The output equations are simply
G(q, z ) = L(q, z ) = q - z = 0 (9.4.2)
The manipulator configuration space is thus
{
X= x = éë y T qT
T
}
z T ùû : W(q, y ) = 0 and L (q, z ) = 0 Ì R 6 (9.4.3)
With L q (q, z ) = I = -L z (q, z ) , the output equations contribute no singularities. For the
input equations,
698
éq + 6s 6cq1 + 30s ù
Wq (q, y ) = ê 1 ú
ëq1 - 5s -5cq1 + 30s û
(9.4.4)
éy 0 ù
Wy (q, y ) = ê 1 ú
ë 0 y2 û
The condition Wy (q, y ) = y1y 2 = 0 yields a singular configuration only if y1 = 0 , since the
geometry of the manipulator precludes y2 = 0 . For the condition
Wq (q, y ) = -11( cq12 - 30s 2 ) = 0 (9.4.5)
singular values are q1 = ± 30 s / c for -p / 2 < q 2 < p / 2 ; i.e., two curves in the q1 -q 2 plane,
30 s
q1p (q 2 ) =
c
-p / 2 < q 2 < p / 2 (9.4.6)
- 30 s
q (q 2 ) =
m
1
c
The only solution of Eq. (9.4.5) in the range p / 2 £ q 2 £ 3p / 2 , where c = cos(q 2 ) < 0 is the
isolated point (0, p ). From the first form of Eq. (9.4.1), if y1 = 0 , then c = 5 / 6 and q1 = -6s , in
which case cq12 - 30s2 = (5 / 6)36s2 - 30s2 = 0 . Thus, the only singularity condition that need be
considered is cq12 - 30s2 = 0 .
For q and y that satisfy Eq. (9.2.14); i.e., cq12 - 30s2 ¹ 0 , from Eq. (9.2.15) , there exists a
unique solution q = f ( y ) of Eq. (9.4.1). Similarly, for q and y that satisfy Eq. (9.2.13); i.e.,
y1 ¹ 0 , Eq. (9.4.1) yields
é q 2 + 12sq - 60c + 61 ù
y = g( q ) = ê ú
1 1
(9.4.7)
êë q1 - 10sq1 - 60c + 61 úû
2
where y i ³ 0 is required. Finally, for any q and z, Eq. (9.4.2) yields, as in Eq. (9.2.14),
z = h(q ) = q . For all q and y that satisfy cq12 - 30s2 ¹ 0 , these relations show that the regular
configuration space can be parameterized by q; i.e.,
X {
% = x = é g T (q ) q T
ë
T
qT ûù : cq12 - 30s2 ¹ 0} (9.4.8)
This is a two-dimensional manifold that is parameterized by q Î R 2 .

One-dimensional singular sets that separate submanifolds of X % are solutions of
cq12 - 30s2 = 0 ; i.e., q1p (q 2 ) and q1m (q 2 ) of Eq. (9.4.6). Plots of q1p (q 2 ) and q1m (q 2 ) are presented
in Fig. 9.4.2. These results provide boundaries of domains of functionality in the q1 -q 2
699
parameterization plane, in the range -p / 2 £ q 2 £ p / 2 rad. The only singular value for q 2 in the
range p / 2 < q 2 < 3p / 2 is the isolated solution of Eq. (9.4.5) at q = [0 p] .
T
Figure 9.4.2 Singular Values q1p = q1p (q 2 ) and q1m = q1m (q 2 )

Combining the result of Fig. 9.4.2 with this result yields three domains of functionality,
shown in the q1 -q 2 parameterization plane in Fig. 9.4.3, analytically represented as
% = {x Î R 6 : -p / 2 < q < p / 2, q p (q ) < q , y = g(q ), z = q}
X 1 2 1 2 1
% = {x Î R 6 : -p / 2 < q < p / 2, q < q m (q ), y = g(q ), z = q}

X 2 2 1 1 2
% = {x Î R 6 : -p / 2 < q < 0, q m (q ) < q < q p (q ), y = g(q ), z = q}

X (9.4.9)
3 2 1 2 1 1 2
U {x Î R 6 : 0 < q 2 < p / 2, q1m (q 2 ) < q1 < q1p (q 2 ), y = g (q ), z = q}

U {x Î R 6 : p / 2 < q 2 < 3p / 2, - ¥ < q1 < ¥, less ( 0, p ) , y = g(q ), z = q }
Configurations of the manipulator in each of the three domains of functionality are shown in Fig.
9.4.4. The extensive domain (3) in Fig. 9.4.3 includes configurations that are reflected about
-p / 2 and 3p/2 on the q 2 axis. The three subsets of R 6 in Eq. (9.4.9) are submanifolds of X% ,
hence they are maximal, path connected, subsets of configuration space X that contain no
singularities.
700
Figure 9.4.3 Domains of Functionality in the q1 -q 2 Parameterization Plane
Figure 9.4.4 Configurations in Domains 1 through 3

It is interesting to note that each of the configurations in Fig. 9.4.4 may be associated
with the desired functionality of the manipulator. This is particularly true of domains (1) and (2).
In real-world applications of manipulators, just one of several possible domains of functionality
generally represents a desired form of functionality. In such cases, the engineer will naturally
wish to focus on that domain and develop information needed to assure the manipulator can be
effectively controlled in that domain.
To confirm that the manipulator can operate in the domain on the right of Fig. 9.4.3
without encountering a singular configuration other than q = [0 p ] , a circle of radius r > 0
T
and center at (0,p ) , q12 + (q 2 - p)2 = r 2 , is traversed using polar coordinate 0 £ f £ 2p ; i.e.,
q1 = rsin f and q 2 = p + r cos f . Associated inputs y1 and y2 are calculated using Eqs. (9.4.1).
The path is traversed with the determinant Wq (q, y ) positive throughout with values of 0<r £ 3
so that the circle does not intersect the singular curves on the left of Fig. 9.4.3. Four points on
this circle in the q1 -q 2 plane are shown in Fig. 9.4.5, with associated configurations of the
manipulator shown in Fig. 9.4.6.
701
%
Figure 9.4.5 Nonsingular Circular Trajectory in X 3
Figure 9.4.6 Manipulator Configurations in Traversing Circle of Fig. 9.4.5

To illustrate a form of behavior that has been presented in the literature (Peidro, Marin,
Gil, and Reinoso, 2015) for multiple manipulator configurations with the same input coordinates,
let y1 = y 2 = y . Subtracting the second of Eqs. (9.4.1) from the first yields an equivalent system
of equations 22sq1 = 0 and q12 - 10sq1 - 60c + 61 - y2 = 0 . Solutions with s = sin(q 2 ) = 0 ; i.e.,
q 2 = 0 or p yield (i) q 2 = 0 , c = 1, and q1 = ± y2 - 1 and (ii) q 2 = p , c = - 1, and
q1 = ± y2 - 121 . With y1 = y 2 = 11.2 , this yields four distinct configurations with the same
input; i.e., assembly modes q = [11.155 0 ] , [ -11.155 0] , [2.107 π ] , and [ -2.107 π ] .
T T T T
The first and second configurations are in domains (1) and (2) of Fig. 9.4.3 and the third and
fourth are in domain (3). There is therefore no nonsingular path between the first or second
configuration and any of the other three. Since the third and fourth configurations are in domain
(3), which is a path connected set, there must be a continuous nonsingular trajectory between
them. In fact, the circular trajectory of Fig. 9.4.5, q12 + (q 2 - p)2 = r 2 with r = 2.107, contains the
702
third and fourth assembly modes and resides entirely within domain (3), providing a continuous
nonsingular trajectory between these configurations.
As was observed in Section 9.3, while the domains of functionality are path connected,
they need not be simply connected. In this example, domain (3) is not simply connected since it
contains an isolated singularity at q = [0 p ] .
T
The elementary planar manipulator of Fig. 9.4.1 illustrates basic properties of the regular manipulator
configuration space and its singularity free domains of functionality. In particular, the domains of
functionality are singularity free and path connected, no nonsingular trajectory can connect points in
distinct domains, multiple configurations with the same input may exist in a domain, and the domains
need not be simply connected.
703
9.5 Front-End Loader
The front end loader introduced in Section 9.1.5 and shown in Fig. 9.5.1 is typical of high
load capacity manipulators used in construction industries.

The mechanism has three bodies that move relative to body 0 (the chassis), whose
positions and orientations are defined in the chassis-fixed x-y frame by three generalized
coordinates shown in Fig. 9.5.1, q = [q1 q 2 q 3 ] . Hydraulic actuator inputs y = [ y1 y2 ]
T T
control motion of the system, and outputs are the orientation and elevation of the bucket,
T
z = éë q1 + q 3 y D 2 ùû . Body 2 slides along the axis of body 1 (the boom) and is connected to point
D1 on body 3 (the bucket) by a bar of length 2 m, to control orientation of the bucket. In this
model, motion of the chassis is suppressed.
9.5.1 Loader Kinematics
Body-fixed vectors that define points of connection and the tip D2 of the bucket are
r A1
= r ¢A1 = -1.5u y , r ¢B1 = ux , r ¢B2 = 4u x , r ¢C1 = q 2u x , r ¢D1 = u y , and r ¢D2 = u x - 0.5u y , where
u x = [1 0] and u y = [0 1] . Orientation transformation matrices for bodies 1 and 3 are
T T
éc -s1 ù éc -s13 ù
A1 = ê 1 ú and A 3 = ê 13 ú , where c1 = cosq1 , s1 = sin q1 , c3 = cosq 3 , s3 = sin q 3 ,
ë s1 c1 û ë s13 c13 û
c13 = cos(q1 + q 3 ), and s13 = sin ( q1 + q 3 ) . Vectors that define key points on the loader, in the x-y
plane, are r B1 = A1u x , r B2 = 4 A1u x , r C1 = q 2 A1u x , r D1 = r B2 + A 3u y = 4 A1u x + A3u y , and
r D2 = r B2 + A 3 (u x - 0.5u y ) = 4 A1u x + A 3 (u x - 0.5u y ) .
The kinematic constraint due to the 2m bar that connects points C1 and D1 is
704
(r D1 - r C1 )T (r D1 - r C1 ) - 4
= ( (4 - q 2 )uTx A1T + u Ty A 3T )( (4 - q 2 )A1u x + A 3u y ) - 4
= (4 - q 2 )2 - 2(4 - q 2 )s3 - 3 = 0
Using the quadratic formula, this is equivalent to (4 - q 2 ) = s3 ± s23 + 3 . From the geometry of
Fig. 9.5.1, 1 £ q 2 £ 3 , so 3 ³ 4 - q 2 ³ 1 . This requires selection of the plus sign, so the kinematic
constraint may be written as
(
Φ(q ) = q 2 - 4 - s3 - s23 + 3 = 0 ) (9.5.1)
Input equations are

é(r B1 - r A1 )T ( r B1 - r A1 ) - y2 ù é3s + 3.25 - y 2 ù
Ψ(q, y ) = ê 1
ú=ê 1 1
ú=0 (9.5.2)
q 2 - y2
ëê q 2 - y2 ûú ë û
where y1 and y2 are nonnegative, and output equations are
é q 1 + q 3 - z1 ù é q 1 + q 3 - z1 ù
Γ(q , z ) = ê T D 2 ú =ê ú =0 (9.5.3)
ë u y r - z 2 û ë4s1 + s13 - 0.5c13 - z 2 û
Jacobians of constraint, input, and output equations for the loader are
Φq (q ) = éë0 1 (1 + (s 2
3 + 3)-1/2s3 ) c3 ùû
é3c 0 0 ù
Ψq (q, y ) = ê 1 ú
ë 0 1 0û
é 1 0 1 ù
Γ q ( q, z ) = ê ú
ë 4c1 + c13 + 0.5s13 0 c13 + 0.5s13 û
Thus,
0 1 (1 + (s 2
3 + 3)-1/2 s3 ) c3
Ωq (q, y ) = 3c1 0 0 = 3 (1 + (s32 + 3)-1/2 s3 ) c1c3 (9.5.4)
0 1 0
where (1 + (s23 + 3) -1/2s3 ) ¹ 0 and
0 1 (1 + (s 2
3 + 3)-1/ 2s3 ) c3
Λ q (q, z ) = 1 0 1 = 4c1 (9.5.5)
4c1 + c13 + 0.5s13 0 c13 + 0.5s13
705
Since Ωq = Λ q = 0 at any configuration with q1 = ±π/2 , q cannot be written as a
differentiable function of either y or z in a neighborhood of any such configuration. Thus, the
formulation cannot be reduced to differentiable input-output form in such neighborhoods.
9.5.2 Domains of Functionality
From Eqs. (9.3.3), (9.5.4), and (9.5.5), domains of functionality are
X% = {x : c > 0 and c > 0} = {x : - π / 2 < q < π / 2, - π / 2 < q < π / 2}
1 1 3 1 3
X = {x : c > 0 and c < 0} = {x : - π / 2 < q < π / 2, π/2 < q < 3π / 2}

%
2 1 3 1 3
(9.5.6)
% = {x : c < 0 and c > 0} = {x : π/2 < q < 3π / 2, - π / 2 < q < π / 2}
X 3 1 3 1 3
% = {x : c < 0 and c < 0} = {x : π/2 < q < 3π / 2, π/2 < q < 3π / 2}

X 4 1 3 1 3
where
T
é 3.25 + 3s1 ,4 - s3 - s32 + 3,q 1 ,4 - s3 - s23 + 3, ù
x = [ y1 , y 2 ,q 1 ,q 2 ,q 3 ,z1 ,z 2 ]
T
=ê ú (9.5.7)
êëq 3 ,q1 + q 3 ,4s1 + s13 - 0.5c13 úû
These four singularity free domains of functionality are two-dimensional manifolds in X Ì R 7 ,

parameterized by q1 and q 3 . They have been evaluated without defining sets of singularities,
which are their boundaries in X.
To assist in visualization, projections of the domains of Eq. (9.5.6) onto the z1 -z2 plane
are presented in Figs. 9.5.2 and 9.5.3. Domains in Fig. 9.5.2 are associated with q1 in the range
-π / 2 < q1 < π / 2 and those in Fig. 9.5.3 are associated with q1 in the range π / 2 < q1 < 3π / 2 .
In domains X % and X % , the loader is in the intended configuration shown in Fig. 9.5.1, whereas
1 2
domains X% and X
% represent unwanted configurations with the boom oriented rearward. Only
3 4
%
in domain X can the loader carry out its scooping action near ground level, as the chassis
1
moves forward and the bucket is loaded in a near horizontal orientation; i.e., with z1 » 0 . The
bucket is then oriented upward (z1 > 0) to contain the load and elevated as the chassis moves
toward the target hauler. The bucket is then oriented downward (z1 < 0) to dump the load into
the hauler. In domains X% through X% the bucket can achieve only positive orientation,
2 4
precluding the material loading function at ground level. Thus, only one of the domains
represents the intended functionality of the loader.
706
% and X2 = X
Figure 9.5.2 Loader Domains X1 = X %
1 2
% and X4 = X
Figure 9.5.3 Loader Domains X3 = X %
3 4
The closures of all four domains intersect along the line q1 = π / 2 and its extension
q1 = -π / 2 or 3π / 2 in the seven-dimensional configuration space. All boundaries of the
domains shown in Figs. 9.5.2 and 9.5.3 are sets of singularities. To determine whether it is
possible to have nonsingular trajectories that connect points in these domains, midpoints in the
ranges of q1 and q 3 that parameterize the domains in Eq. (9.5.6) are presented in Table 9.5.1 to
evaluate determinants in Eqs. (9.5.4) and (9.5.5). Since no pair of points in Table 9.5.1 have the
same signs of Ωq (q, y ) and Λ q (q, z ) , no points in the associated domains can be connected by
a continuous trajectory in configuration space without encountering a singularity, consistent with
the theory.
707
Table 9.5.1 Ωq (q, y ) and Λ q (q, z )
%
X Point q1 q3 Ωq (q, y ) Λ q (q, z )
i
%
X (1) 0 0 3 4
1
%
X (2) 0 π -3 4
2
%
X (3) π 0 -3 -4
3
%
X (4) π π 3 -4
4
9.5.3 Domains of Functionality with Input Bounds

All domain boundaries in Figs. 9.5.2 and 9.5.3 represent singular configurations in which
either q1 = ±π / 2 (the boom is vertical) or q 3 = ±π / 2 (the link between points C1 and D1 is
colinear with the x1¢ axis). Since these configurations involve bifurcation, they must be strictly
avoided in operation of the loader. To avoid these singular configurations, design limits
0.5 < y1min < y1 < y1max < 2.5 and 1 < y2 min < y2 < y2 max < 3 on inputs are imposed. To keep q1 in
the range -π / 4 < q1 < π / 4 , hence avoiding singularities at q1 = ±π / 2 , bounds on y1 are
selected as y1min = 1.3 and y1max = 2.1 . To avoid bucket linkage singularities at q 3 = ±π / 2 ,
bounds on y2 are selected as y2 min = 1.2 and y2 max = 2.8 . These bounds are enforced with the
transformation of Eq. (9.3.5) to new input coordinates v i , so that y1 (v1 ) = 1.7 + 0.4sin v1 and
y2 (v 2 ) = 2 + 0.8sin v2 . The input equation of Eq. (9.5.2) thus becomes
é3s + 3.25 - y12 (v1 ) ù

Ψ (q, v ) = ê 1 ú=0 (9.5.8)
ë q 2 - y 2 (v 2 ) û
and its Jacobian with respect to v is
é -0.8y1 (v1 )cos v1 0 ù
Ψ v (q, v ) = ê
-0.8cos v 2 úû
(9.5.9)
ë 0
with determinant
Ψ v (q, v ) = 0.64y1 (v1 )cos v1 cos v 2 (9.5.10)
Since y1 (v1 ) ¹ 0 and cosv1 ³ 0 £ cosv 2 , due to the definition of Eq. (9.3.5), the domains of
functionality of Eqs. (9.3.9) become
% ¢ = {x Î X ¢ : cosv > 0 and cosv > 0, throughout}
X (9.5.11)
i 1 2
where
{
X ¢ = x = ëé v T qT
T
z T ûù : Φ(q ) = 0, Ψ(q, v ) = 0, and Γ(q, z ) = 0 } (9.5.12)
708
Since cos vi > 0 for all v i in the range -π / 2 < vi < π / 2 , the domains of Eq. (9.5.11) are simply
% ¢ = {x Î X¢ : - π / 2 < v < π / 2 and - π / 2 < v < π / 2, throughout}
X (9.5.13)
i 1 2
the number of domains being the multiplicity of solutions for x (v ) Î X ¢ , defined by Eqs. (9.5.12)
and (9.5.13) as
y1 (v1 ) = 1.7 + 0.4sin v1
y 2 (v 2 ) = 2 + 0.8sin v 2
s1 (v1 ) = (y12 (v1 ) - 3.25)/3
q1 (v1 ) = Arcsin s1 (v1 ) or π - Arcsin s1 (v1 )
q 2 (v 2 ) = y 2 (v 2 ) (9.5.14)
s3 (v 2 ) = ( (4 - q 2 (v 2 ))2 - 3) / ( 2(4 - q 2 (v 2 )) )
q 3 (v 2 ) = Arcsin s3 (v 2 ) or π - Arcsin s3 (v 2 )
z1 (v ) = q1 (v1 ) + q 3 (v 2 )
z2 (v ) = 4s1 (v1 ) + sin (q1 (v1 ) + q 3 (v 2 )) - 0.5cos(q1 (v1 ) + q 3 (v 2 ))
There are two values for each of q1 (v1 ) and q 3 (v3 ) in Eq. (9.5.14), hence four values of x(v) for
each value of v that satisfies Eq. (9.5.13). Plots of the resulting four input bounded loader
domains of functionality, projected onto the z1 -z2 plane, are presented in Figs. 9.5.4 and 9.5.5.
% ¢ and X2 = X
Figure 9.5.4 Loader Domains X1 = X % ¢ , y-Bounds
1 2
709
% ¢ and X4 = X
Figure 9.5.5 Loader Domains X3 = X % ¢ , y-Bounds
3 4
Closures of these domains of functionality with input-bounds do not intersect, in contrast

with the case in which no input-bounds are enforced. The boundaries of the modified domains
are in fact not kinematic singularities. They are sets for which the Jacobian of Eq. (9.5.10) is
singular, defining configurations in which one of the inequalities in Eq. (9.3.5) is an equality;
i.e., an actuator is at one of its bounds. As in the previous case, the loader can carry out its
intended function only in X % ¢ . In this domain, no kinematic singularity occurs, even at the
1
boundary, so programming and control can be carried out throughout the closure of the domain.
An approach such as this is essential in design of industrial quality manipulators, freeing the
control system engineer from the responsibility of avoiding disastrous near singular
configurations that exist in the unconstrained workspace.
Criteria for singularity free domains of functionality for the front-end loader yield four such domains,
only one of which practically meets loading and dumping requirements. This suggests the need for
numerical methods that focus on the one practical domain to define its extent and boundary. The more
conservative and practical formulation that places bounds on inputs to uniformly avoid singular
configurations is shown to lead to a restricted single domain of functionality that meets all functionality
requirements.
710
9.6 A Model Manipulator
While invariance of the signs of determinants of matrices that appear in Eq. (9.3.3),
throughout each domain of functionality X% , assures manipulator functionality over each such
i
domain, it does not provide a means for defining the extent of these domains in X Ì R n + 2 k . A
model manipulator of higher dimension is helpful in extending the analysis presented.
9.6.1 Manipulator Coordinates and Equations
A bar of unit length, with a vertical extension at its outboard end, is pivoted with a
revolute joint about the global z axis and rotates in the x-y plane with angle q1 relative to the x
axis, as shown in Fig. 9.6.1. A translational joint on the vertical extension enables translation of a
coupler, shown in black, parallel to the z axis with coordinate q 2 above the x-y plane. A
connecting rod of unit length connects the coupler to a body that moves in a translational joint
along the y axis, with coordinate q 3 . These mechanical components and generalized coordinates
q1 , q 2 , and q 3 comprise a two degree of freedom mechanism. Actuators with lengths
y1 > 1 and y2 > 1 control distances of the coupler from points (2,0,0) and (0,2,0) in the x-y-z
frame. Finally, a telescoping straight member, shown as a dashed line in Fig. 9.6.1, is connected
to the outboard end of the rotating bar and to the body that translates along the y axis. The
manipulator output is the coordinates z1 and z2 of the midpoint of the telescoping member in the
x-y plane.
Figure 9.6.1 Spatial Parallel Manipulator

Input, generalized, and output coordinates for this manipulator are y = [ y1 y2 ] Î R 2 ,
T
q = [q 1 q 2 q 3 ] Î R 3 , and z = [ z1 z 2 ] Î R 2 . Manipulator coordinates are thus

T T
T
x = ëé y T qT z T ùû Î R 7 . Defining c º cosq1 and s º sin q1 , the constraint that enforces unit
length of the connecting rod is
Φ(q ) = (1 / 2) ( c 2 + (q 3 - s)2 + q 22 - 1) = (1 / 2) ( q 32 - 2sq 3 + q 22 ) = 0 (9.6.1)
Input equations that define lengths y1 and y2 of the actuators as functions of q are
711
é( (c - 2)2 + s2 + q 22 - y12 )ù é( -4c + q 22 + 5 - y12 ) ù
Ψ(q, y ) = (1 / 2) ê ú = (1 / 2) ê ú=0 (9.6.2)
ê( c2 + (s - 2)2 + q 22 - y22 )ú ê ( -4s + q 22 + 5 - y22 ) ú
ë û ë û
and output equations that define z1 and z2 as functions of q are
é c - 2z1 ù
Γ( q, z ) = ê ú=0 (9.6.3)
ëq 3 + s - 2z 2 û
The manipulator configuration space is thus
X = {x : Φ(q ) = 0, Ψ(q, y ) = 0, and Γ(q, z ) = 0} Ì R 7 (9.6.4)
Note that q 2 , y1 , and y2 appear as their squares in Eqs. (9.6.1) through (9.6.3). Only
nonnegative values of the yi are allowed, but both positive and negative values of q 2 must be
anticipated.
Jacobians of the functions defined in Eqs. (9.6.1) through (9.6.3) are
Φq (q ) = [ -cq 3 q2 (q 3 - s)]
é 2s q2 0ù
Ψ q ( q, y ) = ê
ë -2c q2 0 úû
é - y1 0 ù
Ψ y ( q, y ) = ê
- y 2 úû
(9.6.5)
ë 0
é -s 0 0ù
Γq (q, z ) = ê
ëc 0 1 úû
Γ z (q, z ) = -2I
and determinants that appear in Eq. (9.3.3) are
é -cq 3 q 2 (q 3 - s) ù
Ωq (q, y ) = ê 2s q 2 0 ú = 2q 2 (q 3 - s)(s + c)
ê ú
ëê -2c q 2 0 úû
é -cq 3 q 2 (q 3 - s) ù
Λ q (q, z ) = ê -s 0 ú = sq 2
(9.6.6)
0
ê ú
êë c 0 1 úû
Ψ y (q, y ) = y1 y2
Γ z (q, z ) = 4
At any configuration with q 2 = 0 , Ωq = Λ q = 0 , so q cannot be written as a

differentiable function of y or z. It is thus not possible to reduce the formulation to differentiable
input-output form in any neighborhood of such configurations.
712
9.6.2 Domains of Functionality
From Eq. (9.6.1), q 3 = s ± s2 - q 22 , so q 3 - s = ± s2 - q 22 ¹ 0 if and only if s2 - q 22 > 0 .

In order that the conditions of Eq. (9.2.14) hold, with determinant values of Eq. (9.6.6) and the
foregoing result, the domains of functionality are
% = {x Î X : s 2 - q 2 > 0, s ¹ 0, s + c ¹ 0, and q ¹ 0 throughout}
X (9.6.7)
i 2 2
The condition y1y2 ¹ 0 is not included, since neither y1 = 0 nor y 2 = 0 can occur. Eight
singularity free domains of functionality, based on Eq. (9.6.7), are
% = {x : s2 - q 2 > 0, s > 0, s + c > 0, and q > 0}
X1 2 2
% = {x : s 2 - q 2 > 0, s > 0, s + c < 0, and q > 0}

X 2 2 2
% = {x : s2 - q 2 > 0, s < 0, s + c > 0, and q > 0}

X 3 2 2
% = {x : s 2 - q 2 > 0, s < 0, s + c < 0, and q > 0}

X 4 2 2
(9.6.8)
% = {x : s2 - q 2 > 0, s > 0, s + c > 0, and q < 0}
X 5 2 2
% = {x : s2 - q 2 > 0, s > 0, s + c < 0, and q < 0}

X 6 2 2
% = {x : s2 - q 2 > 0, s < 0, s + c > 0, and q < 0}

X 7 2 2
% = {x : s2 - q 2 > 0, s < 0, s + c < 0, and q < 0}

X 8 2 2
where, from Eqs. (9.6.1), (9.6.2), and (9.6.3),

T
é q 22 + 5 - 4c, q 22 + 5 - 4s,q1 ,q 2 ,ù
x = [y1 , y 2 ,q1 ,q 2 ,q 3 ,z1 ,z 2 ] = ê
T
ú (9.6.9)
ês ± s2 - q 22 ,c / 2,(q 3 + s)/2 ú
ë û
In each domain of Eq. (9.6.8), q 3 = s ± s2 - q 22 , so there are in fact 16 domains, numbered so

% and X
that X % are symmetric relative to the plane q = 0, X % above the plane, for i = 1,L ,4 .
i i +4 2 i
Inequalities that dictate ranges of values of q1 in Eq. (9.6.8) are
s > 0 and s + c > 0 Û 0 < q1 < 3π/4

s > 0 and s + c < 0 Û 3π/4 < q1 < π
(9.6.10)
s < 0 and s + c > 0 Û - π / 4 < q1 < 0
s < 0 and s + c < 0 Û - π < q1 < - π / 4
Similarly, inequalities that dictate ranges of values of q 2 in Eq. (9.6.8) are
713
s > 0, q 22 < s2 , and q 2 > 0 Û 0 < q 2 < s
s > 0, q 22 < s2 , and q 2 < 0 Û -q 2 < s and q 2 < 0 Û -s < q 2 < 0
(9.6.11)
s < 0, q 22 < s2 , and q 2 > 0 Û q 2 < -s and q 2 > 0 Û 0 < q 2 < -s
s < 0, q 22 < s2 , and q 2 < 0 Û -q 2 < -s and q 2 < 0 Û s < q 2 < 0
With Eqs. (9.6.10) and (9.6.11), singularity free domains of functionality of Eq. (9.6.8) are
% = {x : 0 < q < 3π / 4, 0 < q < s}
X1± 1 2
% = {x : 3π / 4 < q < π, 0 < q < s}

X 2± 1 2
% = {x : - π / 4 < q < 0, 0 < q < -s}

X 3± 1 2
% = {x : - π < q < - π / 4, 0 < q < -s}

X 4± 1 2
(9.6.12)
% = {x : 0 < q < 3π / 4, - s < q < 0}
X 5± 1 2
X 6 ± = {x : 3π / 4 < q1 < π, - s < q 2 < 0}

%
% = {x : ( - π / 4 < q < 0, s < q < 0 )}
X 7± 1 2
% = {x : - π < q < - π / 4, s < q < 0}

X 8± 1 2
where x is defined in Eq. (9.6.9) and the plus and minus signs are those in q 3 = s ± s2 - q 22 .
Each of these domains is a two-dimensional subset of R 7 , parameterized by q1 and q 2 .
Unlike the slider-crank, it is not possible to draw pictures of domains in such a high
dimensional space. Nevertheless, Eqs. (9.6.12) explicitly define 16 singularity free domains of
functionality, in which reliable programming and control of the manipulator may be carried out;
i.e., these are sets in which the manipulator programmer and control system designer may
operate without encountering singularities.
Complicated as these domains are for programming and control, an alternative that is
advocated in the literature on manipulator singularities is to find all singular sets in X and invite
the programmer and control system designer to find his/her way through such a maze of
singularities to define trajectories that connect desired configurations. In this singularity maze
setting, there is no assurance that such trajectories exist. In the domain of functionality setting of
Eq. (9.6.12), any pair of configurations in a domain X % can be connected by a nonsingular
i±
trajectory. This is guaranteed by results of Section 9.3. Like it or not, the same theory guarantees
that any trajectory between configurations in different domains must contain a singularity.
9.6.3 Boundaries of Domains of Functionality as Sets of Singularities
Projections of the first four domains of functionality of Eq. (9.6.12) onto the z1 -z2 output
plane are shown in Fig. 9.6.2 as X1 ± and X2 ± , where + and - correspond to the algebraic
signs in Eqs (9.6.9) and (9.6.12). Sets in the projection onto the output space actually represent
two domains each, one with q 2 > 0 and one with q 2 < 0 , both with the same projections onto
the z1 -z2 plane. Curves that bound these domains are sets of singularities, even though they were
not independently computed as such. Each of the 16 domains has three singular boundary
segments in R 7 , for a total of 48 one-dimensional sets of singularities in seven-dimensional
714
space. Even if these sets could be pieced together to define boundaries of the 16 domains of
functionality, the analytical form of the domains that is provided by Eq. (9.6.9) is not defined by
the singular sets.
Figure 9.6.2 Domains of Functionality Projected onto the Output Space

% and X
In the context of Fig. 9.3.1, the closures of domains X % above the q = 0 plane
1± 2± 2
% %
and above z = 0 in Fig. 9.6.2, as well as X and X below z = 0 , intersect along common
2 3± 4± 2
boundaries. In contrast, closures of domains above z2 = 0 intersect with closures of domains in

the lower half of Fig. 9.6.2 only at isolated points a and b. The same is true of the eight domains
% through X
X % below the q = 0 plane.
5± 8± 2
Values of z and associated mechanism generalized coordinates of points denoted (1)

through (4) in the upper half of Fig. 9.6.2, defined by midpoint values of q1 and q 2 in
% and X
X % in Eq. (9.6.12), are shown in Table 9.6.1. These data confirm that the sign of
1± 2±
Ωq (q, y ) differs across common boundaries of the domains of functionality identified in Table
9.6.1. The same is true of domains in the lower half of Fig. 9.6.2 and all domains below the
q 2 = 0 plane. This is consistent with the prediction that any continuous trajectory with endpoints
in different domains of functionality must encounter a singularity.
715
%
X Point z1 z2 q1 q2 q3 Ωq (q, y ) Λ q (q, y )
i
%
X (1) 0.1913 0.5625 3π / 8 0.4619 0.1291 - 0.9593 0.4267
1-
%
X (2) 0.1913 1.3240 3π / 8 0.4619 1.724 0.9657 0.4267
1+
%
X (3) -0.4619 0.2170 3π / 8 0.1914 0.5127 0.06867 0.07325
2-
%
X (4) -0.4619 0.5484 3π / 8 0.1914 0.7141 - 0.06867 0.07325
2+
The foregoing analysis verifies theoretical predictions regarding properties of domains of

functionality, but suggests difficulties in implementation using only singularity information, as is
advocated in the literature on manipulator singularities. The intricate process of computing 48
one-dimensional singular sets and piecing them together in seven-dimensional configuration
space to define boundaries of 16 domains of functionality is problematic, for even this modest
manipulator. With evaluation of a large number of (k – 1)-dimensional sets of singular
configurations in X Ì R n + 2 k , for k > 2, it is not clear how to identify and characterize the
likewise large number of domains X % in which the manipulator will function reliably. Even with
i
a knowledge of (k – 1)-dimensional singular boundary sets in (n + 2k)-dimensional configuration
space, for realistic systems with n ³ k and n + 2k >> 7 , piecing them together to define
boundaries of domains of functionality is akin to solving a jigsaw puzzle in (n + 2k)-dimensional
space, a process that is unlikely to succeed. Even if successful, such a process defines only the
boundary of domains of functionality, not the domains themselves that are defined in Eq. (9.6.9).
The approach that focuses on the domains X % presented here, rather than on large numbers of
i
sets of singular configurations, is clearly preferable.
The model problem of Fig. 9.6.1 illustrates use of criteria that define singularity free domains of
functionality in a seven-dimensional manipulator configuration space. The fact that this modest
application has 48 distinct sets of singularities that bound 16 domains of functionality serves as a
warning that real world applications are likely not analytically solvable. It further casts doubt that the
singularity maze concept of finding one’s way through manipulator configuration space is practical. It
suggests that computational methods are needed to characterize domains of functionality and may serve
as a test problem for such methods.
716
9.7 Computation of Domain Boundaries
For matrices of low dimension, analytical criteria such as those employed in Sections 9.3
through 9.6 can be implemented using the determinant to characterize rank and rank deficiency.
For higher dimensional applications, the determinant is not the preferred alternative. This is
especially the case when numerical methods are employed that begin with a point x interior to
% , move along a selected vector to a point on the boundary, and proceed to map the boundary
X i
of X % .
i
9.7.1 Criteria for Boundaries of Domains of Functionality

A practical criterion for rank deficiency of an m ´ n matrix A, with m £ n , is existence
of a vector x Î R m such that
ATx = 0, xTx = 1 (9.7.1)

This criterion has been used in mapping boundaries of manipulator workspaces and sets of
singular configurations (Haug, Luh, Adkins, and Wang, 1996; Bohigas, Montserrat, and Ros,
2017).
A necessary and sufficient condition for the rank of an m ´ n matrix A, with m £ n , is
the number of nonzero singular values of A (Bernstein, 2009). Equivalently, the rank of A is the
number of nonzero eigenvalues of the positive semidefinite matrix AA T (Atkinson, 1989). The
eigenvalues of AA T are denoted z i (AA T ) ³ 0, i = 1,L , m , ordered such that
z m ³ z m -1 ³ L ³ z1 ³ 0 . The regular configuration space of Eq. (9.2.14) may thus be
characterized as
ìï æ z1 (Wq (q , y )WqT (q, y )), z1 (Yy (q, y )YyT (q, y )), ö üï
%
X = í x Î X: min ç ÷ > 0 ý (9.7.2)
ç z (L (q, z )LT (q , z )), z (G (q , z )G T (q , z )) ÷
ïî è 1 q q 1 z z ø ïþ
Since the smallest eigenvalue of a matrix whose elements are continuous functions of x is a
% is open in R n + 2 k , with boundary
continuous function of x (Kato, 1980), the set X
ì there is a w Î X % with w Î B(x, d) for arbitrarily small d and ü
i
ïï ïï
% = íx Î X:
¶X æ z1 (Wq (q, y )WqT (q, y )), z1 (Yy (q, y )YyT (q, y )), ö ý (9.7.3)
ï min ç ÷ = 0 ï
ç z (L (q, z )LT (q, z )), z (G (q, z )G T (q, z )) ÷
ïî è 1 q q 1 z z ø ïþ
This criterion has been used in analysis of manipulator singularities by Ghosal and Ravani
(2001).
9.7.2 Approximating Boundaries of Domains of Functionality
Instead of mapping the exact boundary of X % , which is fraught with rank deficiency
i
complexities, the boundary may be approximated by setting the absolute value of a criterion that
is zero on the boundary equal to a small positive quantity, thus defining a (k - 1) dimensional set
717
% that approximates ¶X
in the interior of X % and avoids rank deficiency. To illustrate such an
i i
% ,
approach, for small e > 0 , define the open subset of X i
{ i q (
% : abs W (q, y ) ´ L (q, z ) ´ Y (q, y ) ´ G (q, z ) > e
X i (e ) = x Î X q y z ) } (9.7.4)
The boundary of this open set is
{ % :
¶X i (e ) = x Î X i }
Wq (q, y ) ´ L q (q, z ) ´ Yy (q, y ) ´ G z (q , z ) = e ´ signpr (9.7.5)
% , which must be constant

where signpr is the sign of the product at an interior point of X i
% %
throughout X . Since ¶X(e) is interior to X , each Jacobian has full rank on ¶X(e) and no
i i
singularities can occur in computation.
Since X i (1/(m+1)) É X i (1/m) , Xi (1/m) is a monotonically increasing sequence of
% (Dontchev and
subsets of X Ì R n + 2 k , m = 1,2,..., lim X i (1/m) = X% i , the closure of X i
m ®¥
% . These results may be used as the basis for
Rockafellar, 2014). Thus, lim ¶X i (1/m) = ¶X i
m ®¥
mapping approximations of boundaries of domains of functionality, using methods of (Haug,

Luh, Adkins, and Wang, 1996; Bohigas, Montserrat, and Ros, 2017).
9.7.3 Boundaries of Domains of Functionality for Model Problem of Section 9.6
The approximation approach of Section 9.7.2 is illustrated for the model manipulator of
Section 9.6. For implementation, the manifold is parameterized using output coordinates and
relations of Section 9.6. Since G z (q, z ) of Eq. (9.6.6) can never be zero, it need not be included
in the computation. Further, the factor q 2 is common to Wq (q, y ) and L q (q, z ) of Eq. (9.6.6),
so distinct factors in the product of Eq. (9.7.5) are
w(q, y ) = y1y 2sq 2 (q 3 - s)(s + c) = y1y 2q 2 (q 3 (s2 + sc) - s3 - s2c) (9.7.6)
Jacobians of w(q, y ) with respect to q and y are
w q = y1y 2 éëq 2 ( q 3 (2sc + c 2 - s2 ) - 3s2c - 2sc 2 + s3 ) q 3 (s2 + sc) - s3 - s2c q 2 (s2 + sc) ùû
w y = q 2 (q 3 (s2 + sc) - s3 - s2c)[ y 2 y1 ]
T
Equation (9.6.2) is explicitly solved for y as y = g(q ) = é 5 + q 22 - 4c 5 + q 22 - 4s ù
ë û
and the Jacobian g ¢( q ) of Eq. (9.2.18) is the solution of Yy g ¢ = -Yq . The nonlinear equation
T
L(q, z ) = éëF T (q ) G T (q, z ) ùû = 0 is iteratively solved for q as a function of z using the Newton-
Raphson method; i.e., L q (q i , z ) Dq i = - L ( q i , z ) , q i +1 = qi + Dqi , yielding the result of Eq.
(9.2.17), q = e( z ) = lim q i , and L q e¢ = - L z .
i ®¥
718
Using these results, w(q, y ) = w(e( z ), g(e( z))) is a function of z and its gradient with
% such that
respect to z is w ¢ = w z = (w q + w y g ¢)e¢ . To map ¶X i (e) , beginning with a point x Î X i
w( z ) > e , steps are taken in the positive z2 direction with step size d > 0 until w( zˆ ) = e and
signw = sign ( w( zˆ ) ) is recorded, since it must remain constant throughout X% . A step is taken in
i
the direction w¢T^ = Pw¢T that is tangent to w( z ) - e ´ signw = 0 , where from Eq. (2.3.29)
é 0 -1ù d
P=ê ú , to z% = zˆ + w ¢T ^ . With w ¢T orthogonal to the curve w( z ) - e ´ signw = 0 ,
ë 1 0 û w ¢T^
w ( z% + xw ¢T ) - e ´ signw = 0 is solved for a new point z = z% + xw¢T on the curve, using Newton-
( )
Raphson iteration; i.e., w ¢w ¢T Dx j = - w ( z% + x j w ¢T ) - e ´ signw , x j+1 = x j + Dx j . This process is
% .
continued until the original point ẑ is reached. The result is ¶X i (e) , an approximation of ¶X i
%
The process is repeated with smaller e , until a satisfactory approximation of ¶X is achieved.i
This algorithm was implemented with d = 0.0001 and a range of values of e . Plots of
approximate boundaries of domains (1) and (2) of Fig. 9.6.2 are shown in Fig. 9.7.1, for three
values of e . Results show convergence to the theoretical result of Fig 9.6.2. The sign of w is
negative throughout domain (1) and positive throughout domain (2), confirming that it is not
possible to continuously move between the domains without encountering a singularity.
Figure 9.7.1 Approximations of Domains (1) and (2), q 2 ³ 0
719
These domain boundaries were computed with q 2 > 0 . With q 2 < 0 , different domain
boundaries were obtained, but with the same projections onto the output space, just as in
theoretical results presented in Section 9.3. Similarly, approximations of domains (3) and (4) of
Fig. 9.6.2 were obtained, as shown in Fig. 9.7.2. The signs of w throughout all four domains
show that trajectories from one domain to an adjacent domain necessarily pass through singular
sets with w = 0.
Figure 9.7.2 Approximations of Domains (3) and (4), q 2 ³ 0

The approach and results presented in this section illustrate the focus on domains of
functionality, rather than on sets of singularities. The objective is to characterize domains in
which the manipulator is controllable and programmable; i.e., functional. Boundaries of such
domains are sets of singularity that are obtained as a biproduct of analysis, not as an independent
objective. This is especially important in realistic manipulator applications that involve many
domains of functionality and even larger numbers of singular boundary subsets.
To implement the foregoing approach for higher dimensional applications, continuation
methods such as those of (Haug, Luh, Adkins, and Wang, 1996; Bohigas, Montserrat, and Ros,
2017) are required to map approximate boundaries of dimension greater than one.
A conservative method for approximation of domains of functionality is presented, beginning at a point

interior to the desired domain, progressing along a vector in configuration space until a point that satisfies
the conservative boundary is reached, and proceeding to map that conservative boundary. A sequence of
such approximations is computed that converges to the boundary of the desired domain. While only one
such possible numerical method, it suggests that even more practical methods may be found.
720
9.8 Manipulator Kinematic Analysis
As shown in Fig. 9.2.1, the mechanical component of a parallel manipulator is a
mechanism that is comprised of rigid bodies whose configuration is defined by generalized
coordinates q Î R n that are subject to n - k ³ 0 holonomic kinematic constraints,
Φ (q ) = 0 (9.8.1)
where k is the dimension of nonredundant input and output coordinates. Input coordinates
y Î R k , with associated actuator forces and torques that are intended to control motion of the
manipulator, are related to mechanism generalized coordinates through k input equations,
Ψ(q, y ) = 0 (9.8.2)
Similarly, output coordinates z Î R k that define manipulator working capability are related to
mechanism generalized coordinates through k output equations,
Γ (q , z ) = 0 (9.8.3)
9.8.1 Multivariable Differential Calculus
Velocity and acceleration analysis require derivatives of functions in Eqs. (9.8.1) through
(9.8.3). A compact differential calculus notation is needed to avoid what would otherwise
become unwieldy equations. Let u Î R m and v Î R n be vector variables and H (u , v ) be a k-
vector of differentiable functions of u and v. Partial derivatives of H (u , v ) with respect to its
arguments are defined as
Hu (u, v ) º H(u, vˆ )u º éë¶H i (u, vˆ ) / ¶u j ùû k ´m
(9.8.4)
H v (u, v ) º H(uˆ , v )v º éë¶H i (uˆ , v ) / ¶v j ùû kń
where a super karat (hat) denotes a variable that is held constant for the indicated differentiation;
i.e., partial differentiation. With mappings a( w ) : R r ® R m and b(x ) : R s ® R n , chain rules of
differentiation are
( H(a( w), b( xˆ )) )w = Hu (a( w), b( x ))a¢( w)
(9.8.5)
( H(a( wˆ ), b( x )) )x = H v (a( w), b( x ))b¢(x )
where
a¢( w ) º éëa i ( w ) / ¶w j ùû m ´r
(9.8.6)
b¢(x ) º éë bi (x ) / ¶x j ùû n´s
For second derivatives that are required in acceleration analysis, define
721
Huu (u, v, a ) º ( H u (u, vˆ )aˆ )u
Huv (u, v, a ) º ( Hu (uˆ , v )aˆ )v
(
H vu (u, v,b) º H v (uˆ , v )bˆ ) u
(9.8.7)
Hv v (u, v, b) º ( H (uˆ , v )bˆ )

v
v
where a Î R m and b Î R n are held constant in differentiation. These derivatives are sufficient
for manipulator kinematics and dynamics, avoiding the need for tensor calculus constructs.
9.8.2 Forward and Inverse Configuration Mappings
As in Section 9.2, composite equations that combine Eqs. (9.8.1) and (9.8.2) for forward
kinematics are
é Φ(q ) ù
Ω(q, y ) º ê ú=0 (9.8.8)
ë Ψ(q, y ) û
and Eqs. (9.8.1) and (9.8.3) for inverse kinematics are
é Φ(q) ù
Λ(q, z ) º ê ú=0 (9.8.9)
ë Γ(q, z )û
T
At a configuration x = éë y T qT z T ùû Î X of Eq. (9.2.8), conditions that assure Eqs. (9.8.1)
through (9.8.3) (i) uniquely determine q and z as functions of input y in a neighborhood of x ,
establishing local forward kinematics, and (ii) uniquely determine q and y as functions of output
z in a neighborhood of x , establishing local inverse kinematics, are defined in Section 9.2 as
Ψ y (q, y ) ¹ 0 (9.8.10)
Γ z (q , z ) ¹ 0 (9.8.11)
é Φq (q ) ù
Ωq (q, y ) º ê ú ¹0 (9.8.12)
ë Ψ q (q , y ) û
é Φ q (q ) ù
Λ q (q , z ) º ê ú ¹0 (9.8.13)
ë Γ q ( q, z ) û
The implicit function theorem of Section 2.2.5 provides a sufficient condition that an
input y uniquely determines the configuration q of a manipulator in a neighborhood of (q , y )
that satisfy Eq. (9.8.8), namely the condition of Eq. (9.8.12); i.e., there is a unique continuously
differentiable solution q = f (y ) of Eq. (9.8.8) in a neighborhood Y of y ; i.e., Ω(f ( y ), y ) = 0 in
Y. A sufficient condition that q determines a unique output z that satisfies Eq. (9.8.3) in a
neighborhood of q = f ( y ) is the condition of Eq. (9.8.11); i.e., there is a unique continuously
differentiable solution z = h(q ) of Eq. (9.8.3) in a neighborhood W of q ; i.e., Γ(q, h (q )) = 0 in
W.
722
In order for an output z to uniquely determine the configuration q of a manipulator in a
neighborhood of ( q, z ) that satisfy Eq. (9.8.9), Eq. (9.8.13) implies that there is a unique
continuously differentiable solution q = e( z ) of Eq. (9.8.9) in a neighborhood Z of z ; i.e.,
Λ (e( z ), z ) = 0 in Z. Finally, Eq. (9.8.10) implies that there exists a unique continuously
differentiable solution y = g(q ) of Eq. (9.8.2) in a neighborhood W of q , where ( q, y ) satisfies
Eq. (9.8.2); i.e., Ψ(q,g( q )) = 0 in W.
While existence of the foregoing mappings is assured, there is little prospect that they can
be written as closed form formulas. Efficient numerical methods for evaluating the mappings are
presented in Section 9.8.4.
9.8.3 Forward and Inverse Velocity Mappings
Since q = f ( y ) is a solution of Eq. (9.8.8), in a neighborhood Y of y , Ω(f (y ), y ) = 0
holds for all y Î Y . Differentiating this identity with respect to y, using the chain rule of
differentiation, Ωq (f (y ), y )f ¢(y ) + Ωy (f (y ), y ) = 0 . Equation (9.8.12) implies Ωq (q, y ) is
nonsingular, so the continuous matrix function Ωq (f (y ), y ) is nonsingular in a neighborhood Y
of y and, for all y Î Y ,
f ¢(y ) = -Ωq-1 (f (y ), y )Ω y (f (y ), y ) (9.8.14)
Similarly, for the solution z = h(q ) of Eq. (9.8.3) in a neighborhood W of q , Γ(q, h (q )) = 0 , for
all q Î W . Differentiating this identity with respect to q, Γ z (q, h(q ))h¢(q ) + Ωq (q, h(q )) = 0 .
Equation (9.8.11) implies that Γz (q, z ) is nonsingular, so the continuous matrix function
Γz (q, h(q)) is nonsingular in a neighborhood W of q and, for all q Î W ,
h¢(q) = -Γ -z 1 (q, h(q))Γ q (q, h(q )) (9.8.15)
Since q = e(z ) is a solution of Eq. (9.8.9) in a neighborhood Z of z , Λ (e(z ), z ) = 0 holds

for all z Î Z . Differentiating this identity with respect to z, Λ q (e(z ), z )e¢(z ) + Λ z (e(z ), z ) = 0 .
Equation (9.8.13) implies Λ q (q, z ) is nonsingular, so the continuous matrix function Λ q (e(z ), z )
is nonsingular in a neighborhood Z of z and, for all z Î Z ,
e¢(z ) = - Λ -q1 (e(z ), z )Λ z (e(z ), z ) (9.8.16)
Likewise, for the solution y = g(q ) in a neighborhood W of q , Ψ(q, g(q )) = 0 holds for all
q Î W . Differentiating this identity with respect to q, Ψ y (q, g(q ))g¢(q ) + Ψq (q, g(q )) = 0 .
Equation (9.8.10) implies Ψ y (q, y ) is nonsingular, so the continuous matrix function
Ψ y (q, g(q )) is nonsingular in a neighborhood W of q and, for all q Î W ,
g ¢(q) = -Ψ -y1 (q, g (q ))Ψq (q, g(q )) (9.8.17)
Using the chain rule of differentiation with Eqs. (9.2.15), (9.2.19), (9.8.14) and (9.8.15)
in forward kinematics,
723
dz
= z y = h¢( f ( y ))f ¢( y )
dy (9.8.18)
-1 -1
= Γ (f ( y ), h(f ( y )))Γq (f ( y ), h(f ( y )))Ω (f ( y ), y )Ω y (f ( y ), y ) º J ( y )
z q
where J ( y ) is called the forward manipulator Jacobian. The time derivative of output, as a
function of input and time derivative of input, is thus the forward velocity mapping,
z& = z y y& = J ( y )y& (9.8.19)
Using the chain rule of differentiation with Eqs. (9.2.17), (9.2.18), (9.8.16), and (9.8.17)
in inverse kinematics,
dy
º y z = g¢(e( z ))e¢(z )
dz (9.8.20)
= Ψ y-1 (e(z ), g(e(z )))Ψq (e(z ), g(e(z )))Λq-1 (e(z ), z )Λ z (e(z ), z ) º K ( z )
where K ( z ) is called the inverse manipulator Jacobian. The time derivative of input, as a
function of output and its time derivative, is thus the inverse velocity mapping,
y& = y z z& = K ( z )z& (9.8.21)
9.8.4 Computation of Forward and Inverse Kinematics
The inverse matrices defined in Section 9.8.3 may be evaluated on domains of
functionality, using a matrix form of the Newton-Raphson method that has been employed in
computational dynamics in Chapter 5. They can then be used to iteratively evaluate solutions of
nonlinear kinematic equations for configuration analysis and to carry out velocity analysis. This
enables computation of inverse matrices at an initial manipulator configuration, as a
preprocessing step, and iteratively refining the inverses in real-time as the manipulator operates.
The process of evaluating Ωq-1 (f ( y ), y ) as a function of y and using the result to evaluate
f ( y ) on a time grid t j is presented in detail. The resulting algorithm may be repeated for
Ψq-1 (q, g(q)) and g(q ) , Γq-1 (q, h(q)) and h(q) , and Λ q-1 (e( z ), z ) and e( z ) .
% at time t , for which (q(t ), y (t )) º (q1 , y1 ) satisfies
At a configuration x (t1 ) º x1 Î X 1 1 1
Eq. (9.8.12), Ω q-1 (q1 , y 1 ) º ( Ω1q )

-1
may be numerically evaluated. On a time grid t1 , t 2 , L, t j in
forward kinematics, y j is a specified input and assume q j = f (y j ) and Ω q-1 (q j , y j ) º ( Ω qj )

-1
have
been evaluated. For y (t ( j+1) ) = y ( j+1) that is near y j in forward configuration analysis; i.e.,
y j - y ( j+1) is small, an estimate for the solution q (j+1) of Eq. (9.8.8) is q(1) = q j , where subscript
in parentheses is iteration number. Newton-Raphson iteration for q( j+1) is begun with
Ωq (q(1) , y ( j+1) )Dq(1) » Ωqj Dq(1) = -Ω(q(1) , y ( j+1) )
724
Since Newton-Raphson iteration does not require an exact inverse of Ωq (q(1) , y ( j+1) ) and
Ωq (q(1) , y ( j+1) ) » Ωqj ,
Dq (1) = - ( Ωqj ) Ω(q(1) , y ( j+1) )

-1
q (2) = q(1) + Dq(1) = q(1) - ( Ωqj ) Ω(q(1) , y ( j+1) )

-1
Continuing,
q (i +1) = q (i) - ( Ωqj ) Ω(q (i) , y ( j+1) ), i = 1, 2,... until Ω( q (i +1) , y ( j+1) ) < Tol1
-1
(9.8.22)
yields an accurate evaluation q( j+1) = f (y ( j+1) ) of Eq. (9.2.15). This is a computationally efficient
algorithm, requiring only evaluation of Ω(q(i) , y ( j+1) ) and multiplication by a constant matrix of
modest dimension. Further, the Newton-Raphson algorithm is quadratically convergent, so only
a few iterations are required.
With q( j+1) known, Ω-q1 (q ( j+1) , y ( j+1) ) must satisfy
R ( Ωq-1 (q( j+1) , y ( j+1) ) ) º Ωq (q( j+1) , y ( j+1) )Ω-q1 (q( j+1) , y ( j+1) ) - I = 0 (9.8.23)
Arguments q( j+1) and y( j+1) are suppressed in the following, since they do not change during
Newton-Raphson iteration for Ωq-1 ( q( j+1) , y ( j+1) ) . Starting with the estimate Ωq-1(1) = Ωq-1 (q j , y j ) ,
Newton-Raphson iteration for Ωq-1 is begun with
R Ω-1
q (1)
(Ω ) DΩ
-1
q (1)
-1
q (1) = Ωq DΩq-1(1) = - R ( Ωq-1(1) ) = -ΩqΩq-1(1) + I
Ωq-1( 2) = Ωq-1(1) + DΩq-1(1)
where subscripts (1) and ( 2 ) are iteration numbers. Since the inverse of Ωq need not be precisely
evaluated for Newton-Raphson convergence, the approximation Ω q-1 » ( Ω qj )

-1
yields
DΩq-1(1) = - ( Ωqj ) Ωq ( Ωqj ) + ( Ωqj )

-1 -1 -1
Ωq-1( 2) = ( Ωqj ) - ( Ωqj ) Ωq ( Ωqj ) + ( Ωqj ) = 2 ( Ωqj ) - ( Ωqj ) Ωq ( Ωqj )

-1 -1 -1 -1 -1 -1 -1
Continuing,
Ω q-1(i +1) = 2 ( Ω qj ) - ( Ω qj ) Ω q ( Ω qj ) , i = 1, 2,... until Ωq-1(i +1) Ωq - I < Tol 2

-1 -1 -1
(9.8.24)
This is a computationally efficient algorithm, requiring only multiplications by matrices of

modest dimension. With Ω-q1 (q ( j+1) , y ( j+1) ) known, subsequent steps in forward configuration
analysis may be carried out on the specified time grid, with the specified inputs y j = y(t j ) .
725
The solution of Eq. (9.8.3) for z( j+1) = h(q( j+1) ) and evaluation of Γ -z 1 (q( j+1) , h(q( j+1) ))
completes forward configuration analysis to t ( j+1) A similar solution of Eq. (9.8.9) for q = e( z )
and evaluation of Λ q-1 (e( z ), z ) and solution of Eq. (9.8.2) for y = g( q ) and evaluation of
Ψ -y1 (q, g(q )) follows the same iterative algorithms for inverse configuration analysis. With
modern, inexpensive, high-speed digital processors, these algorithms for forward and inverse
configuration analysis can be carried out in real-time.
9.8.5 Real-Time Computation
For most applications, the dimension of the manipulator configuration space is 10 to 50,
in which case matrices encountered in the foregoing analysis will fit into the L1 or L2 cache of
modern CPUs. The typical L2 cache size is between 256 and 512 KB per core, which means that
it is likely that all data needed to carry out a kinematic analysis will fit in the L2 cache memory
of one core. As such, any matrix-vector multiplication will benefit from data latencies of the
order of 10 to 20 ns and very high bandwidths. Moreover, Intel AVX512 technologies can
vectorize these matrix-vector multiplications to the point where in one clock cycle one can
perform simultaneously, 8 in double precision or 16 in single precision, fused multiply-add
operations; i.e., 16 and 32 operations, respectively, per clock cycle, with a clock cycle typically
in the vicinity of 0.5 ns. Thus, when the dimension of the configuration space is 10 to 50, one
may expect to carry out kinematic analysis in real-time on a 0.001 sec time grid, with a modern
microprocessor.
The author thanks Professor Dan Negrut of the University of Wisconsin for the foregoing
real-time computation assessment.
While existence of solutions of nonlinear equations and inverses of matrices that vary with generalized
coordinates and time is assured by the implicit function theorem, it does not specify how to evaluate them.
Efficient iterative numerical methods for solution of nonlinear equations of manipulator kinematics are applied to
solve the equations and iterative methods are presented for determining inverse matrices that are needed in
solution of nonlinear equations and in kinematic velocity and acceleration analysis.
726
9.9 ODE of Manipulator Dynamics Embedded in Domains of Functionality
Equations of motion for a manipulator are first written in terms of mechanism
generalized coordinates q, applied forces, and actuator forces that are intended to control motion
of the manipulator, using the d’Alembert variational formulation of Section 4.6,
dq T [ M(q )q
&& - S(q, q& )] - δq TQq (q,q& ,t) - δz TQ z (z,z& , t) - δy TFy (y , t) = 0 (9.9.1)
which holds for all dq that satisfy Φq (q )δq = 0 and associated δy , and δz that satisfy
linearized forms of Eqs. (9.8.2) and (9.8.3). The first term in Eq. (9.9.1) accounts for kinetics of
the mechanism, where M(q ) is its mass matrix and S(q, q& ) contains velocity coupling terms,
sometimes called Coriolis forces. The generalized force Qq (q,q& , t) accounts for external forces
and torques that act on the mechanism, including gravity, friction, and any compliant
components of the mechanism. The generalized force Qz (z,z& ,t) accounts for forces and torques
that resist motion of the output component, or end effector, of the manipulator. The input
generalized force Fy (y, t) is comprised of actuator forces and torques associated with input
coordinates that are exerted to control motion of the manipulator.
Each domain of functionality X% of Eq. (9.3.3) is a maximal, path connected manifold
i
% can be
that can be parameterized by either input y or output z, so equations of motion on each X i
written in terms of y or z. Since control of a manipulator is exerted via forces and torques
associated with input y, it seems most reasonable to write equations of motion in terms of y.
Equation (9.9.1) is thus to be transformed, using results of Section 9.8, to a variational equation
in input coordinates y and subsequently to an ODE in y.
Virtual work terms δqTQq (q,q& , t) and δz TQz (z,z& , t) of Eq. (9.9.1), associated with
generalized coordinates q and output coordinates z, respectively, must be transformed to virtual
work in terms of y, y& , and dy . Differentiating q = f ( y ) with respect to time and taking its
variation,
q& = f ¢( y )y&
(9.9.2)
δq = f ¢( y )δy
so
dqTQq (q, q& , t) = dy Tf ¢T (y )Qq ( f (y ), f ¢(y )y& ,t) (9.9.3)
Similarly, from z = h( q ) and using Eq. (9.9.2)
z& = h¢( f ( y ))f ¢( y )y&
(9.9.4)
δz = h¢( f ( y ))f ¢( y )δy
so
dz TQz ( z, z& , t) = dy Tf ¢T (y )h¢T (f (y ))Qz (h(f (y )), h¢( f (y ))f ¢(y )y& , t) (9.9.5)
Adding virtual work contributions of δy T Fy (y,t) and Eqs. (9.9.3) and (9.9.5),
727
δqTQq (q,q& , t) + δz TQ z (z,z& , t) + δy T Fy (y ,t)dy T
é f ¢T (y )Qq (f (y ), f ¢(y )y& , t) ù
=ê T ú + dy T Fy ( y , t) (9.9.6)
ë +f ¢ (y )h¢ (f (y ))Q (h(f (y )), h¢(f (y ))f ¢(y )y& , t) û
T z
º dy TQqz ( y, y& , t) + dy T Fy ( y , t)
Inertial terms in Eq. (9.9.1) are next written in terms of y and its derivatives. First,
dqTS(q, q& ) = dy Tf ¢T (y )S(f (y ), f ¢(y )y& ) (9.9.7)
The more challenging term involving q && requires use of the chain rule of differentiation with
q& = f ¢(y )y& = -Wq (f (y ), y )Wy (f (y ), y )y& of Eq. (9.8.14). Differentiating this with respect to t,
-1
ì( Wq-1 (f (y ), y )aˆ ) ü
ï y ï
&& = f ¢(y )&&
q y-í ý y&
î
-1
ë
( q
) (
ï+Wq (f (y ), y ) éê Wy (q, yˆ )y&ˆ f ¢(y ) + Wy (f (yˆ ), y )y&ˆ ùú ï
y ûþ
) (9.9.8)
º f ¢(y )&&
y + W( y , y& )
where a º Wy (f (y ), y )y& . Differentiating the identity Wq (f (y ), y )Wq-1 (f (y ), y )a = a , with respect to

y, for an arbitrary constant vector a, Wq (f (y ), y ) ( Wq-1 (f (y ), y )aˆ ) + ( Wq (f (y ), y )Wq-1 (qˆ , yˆ )aˆ ) = 0 ,
y y
y êë ( q
) ( yú
û
)
so ( Wq-1 (f (y ), y )aˆ ) = -Wq-1 (f (y ), y ) é Wq (q, yˆ )bˆ f ¢(y ) + Wq (f (yˆ ), y )bˆ ù , where b º Ω q-1 (q, y )a .
&
Thus, W( y, y ) of Eq. (9.9.8) is
ïìWqq (f (y ), y , b )f ¢(y ) + Wq y (f (y ), y , b ) ïü
W( y , y& ) = Wq-1 (f (y ), y ) í ý y& (9.9.9)
îï -Wy q (f (y ), y , y& )f ¢(y ) - Wy y (f (y ), y , y& ) þï
where the second derivative notation of Eq. (9.8.7) is used. This result is evaluated using
Wq-1 (f (y ), y ) and f ( y ) that are computed with the iterative algorithms of Section 9.8.4, f ¢( y ) of
Eq. (9.8.14), and partial derivatives Ωqq (f (y ), y , b ) , Ωqy (f (y ), y , b ) , Ω yq (f (y ), y , y& ) , and
Ω y y (f (y ), y , y& ) that depend on the specific manipulator.
The variational equation of motion of Eq. (9.9.1) thus reduces to
é f ¢T (y )M( f (y ))f ¢(y )&&
y + f ¢T (y )M(f (y ))W( y, y& ) - f ¢T (y )S(f (y ), f ¢(y )y& )ù
dy ê
T
ú=0 (9.9.10)
ë - Qqz (y , y& , t) - Fy ( y ,t) û
Equation (9.9.1), hence Eq. (9.9.10), must hold for all dq that satisfy Fq (q )dq = 0 and associated
δy . Writing Eq. (9.8.14) in the form Wq (f (y ), y )f ¢(y ) = -Wy (f (y ), y ) , or in component form,
é Fq (f (y )) ù é 0 ù
ê Y (f (y ), y ) ú f ¢(y ) = - ê Y (f (y ), y ) ú (9.9.11)
ë q û ë y û
728
so Fq (f ( y ))f ¢(y ) = 0 . With dq = f ¢(y )dy , Fq (q)δq = Fq (q )f ¢(y )dy = 0 for arbitrary δy . Thus,
Eq. (9.9.10) holds for arbitrary dy , yielding the second order ODE of motion,
f ¢T (y )M(f (y ))f ¢(y )&&

y + f ¢T (y )M(f (y ))W( y, y& ) - f ¢T (y )S(f (y ), f ¢(y )y& )
(9.9.12)
- Qqz ( y, y& ,t) - Fy ( y,t) = 0
To show that Eq. (9.9.12), with associated initial conditions, is a well posed ODE initial-
value problem, it must be shown that the matrix f ¢T (y )M( f (y ))f ¢(y ) is nonsingular. It is shown
in Section 4.6.2 that M ( q ) is positive definite on the null space of Fq (q) , which has full rank in
% ; i.e., a T M(q )a > 0 for all a ¹ 0 such that F (q )a = 0 . From Eq. (9.9.11),
Xi q
Fq (f (y ))f ¢(y )y& = 0 for all y& , so y& f ¢ (y )M(f (y ))f ¢(y )y& > 0 for all y& ¹ 0 and the matrix
T T
f ¢T (y )M( f (y ))f ¢(y ) is positive definite, hence nonsingular. Thus, with initial conditions that
specify y(t 0 ) and y& (t 0 ) , the second order ODE of Eq. (9.9.12) is well posed; i.e., it has a unique
solution that is a continuous function of t, the initial values y(t 0 ) and y& (t 0 ) , and any parameters
on which terms in Eq. (9.9.12) are continuously dependent (Teschl, 2012).
While the initial-value problem of Eq. (9.9.12) and associated initial conditions is well
posed in each of the open domains of functionality X% of Eq. (10.3.3), on their boundaries in
i
X Ì R n + 2 k , one or more of the rank conditions of Eqs. (9.8.10) through (9.8.13) is violated. If
Wq (q, y ) is singular, f ¢(y ) does not exist and Eq. (9.9.12) is meaningless. If G z (q, z ) is singular,
h ¢( q ) does not exist and generalized forces associated with the output coordinates do not exist. It
is not surprising that, just as manipulator functionality is lost on the boundaries of domains X % ,
i
manipulator equations of motion are singular on these boundaries. Thus, both the mathematics of
kinematics and the physics of dynamics dictate that these boundaries be avoided.
The foregoing shows that the kinematics formulation of Section 9.8 yields ODE of
manipulator dynamics that are embedded in the X % , without ad-hoc evaluation of Lagrange’s
i
equations, Newton-Euler equations, or Lagrange multiplier-based DAE. Further, the formulation
assures well posed behavior of the equations of motion, throughout each domain of functionality.
This contributes to manipulator control system design, but requires numerical evaluation of
terms such as f ( y ), h(q ) , f ¢( y ) , Ω q-1 (f (y ), y ) , and Γ -z 1 (f ( y ), h(f ( y ))) . Analytical evaluation of
such terms, even for simple systems, is unlikely. Using iterative methods of Section 9.8.4, as
outlined in Section 9.8.5, evaluation of the equations of motion can be carried out in real-time
with modern high-speed microprocessors.
Using forward manipulator kinematic relations on a manifold of singularity free functionality with the
variational form of system equations of motion in Chapter 4, ODE of parallel manipulator dynamics
are obtained in terms of input coordinates. These equations are embedded in domains of functionality
and are subject to no singularities in each such domain.
729
9.10 Parallel Manipulator Equations of Motion
The ODE formulation of manipulator dynamics of Section 9.9 is used to obtain equations
of motion for the planar front-end loader and a spatial Stewart platform.
The front-end loader defined in Section 9.1.5, with domains of functionality presented in
Section 9.6, is comprised of a planar mechanism and hydraulic actuators.
9.10.1.1 Derivatives Required for Equations of Motion
First order derivatives required for the equations of motion are defined in Section 9.5.1,
é0 1
ê
(1 + (s 2
3 + 3)-1/2 s3 ) c3 ù
ú
Ωq (q ) = ê3c1 0 0 ú
ê0 1 0 ú
ëê ûú
é 0 0ù
Ω y ( y ) = ê -2y1 0ú
ê ú
ëê 0 -1ûú
é 1 0 1 ù
Γ q (q ) = ê ú
ë 4c1 + c13 + 0.5s13 0 c13 + 0.5s13 û
Γz = -I2
The inverse Ωq-1 is computed using the algorithm of Section 9.8.4 and Γ -z 1 = -I 2 . All first order
derivative matrices needed are defined above, enabling evaluation of f ¢(y ) = -Ω q-1 (f (y ))Ω y (y )
and h¢(q) = Γq (q) . The only nonzero second derivative matrices required are
é0 0 b3 ( -s3 - (s32 + 3)-3/2 s3c23 + (s32 + 3)-1/ 2 (c 23 - s32 ) )ù

ê ú
Ωqq (q, b ) = ê0 0 -3b1s3 ú
ê0 0 0 ú
êë úû (9.10.1)
é 0 0ù
Ω y y ( y& ) = ê -2y& 1 0ú
ê ú
ëê 0 0 úû
Thus, from Eq. (9.9.9),
W( y , y& ) = Wq-1 (q ) {Wqq ( q, b)f ¢(y ) - Wy y ( y& )} y& (9.10.2)
where b = Ω q-1 (q )Ω y (y )y& and q = f ( y ) .
730
9.10.1.2 Equations of Motion for the Front-End Loader
For the planar front-end loader mechanism, vectors ri (q ) locate the origins of body fixed
noncentroidal x ¢i -y¢i frames and angles fi (q) define their orientation relative to the inertial x-y
frame in Fig 9.5.1. Time derivatives and variations of ri (q ) and fi (q) , as functions of q and its
time derivatives and variations are r&i = ri¢(q )q& , && && + ri¢(q )qˆ& q& º ri¢(q )q
ri = ri¢(q )q (
&& + ri¢¢ (q, q& ) ,) q
f&i = fi¢(q )q& , && ( )

q + fi¢(q )q&ˆ q& º fi¢¢(q, q& ) , dri = ri¢(q )dq , and dfi = fi¢(q )dq . Terms in the
fi = fi¢(q)&&
q
variational equation of motion for body i, suppressing applied and constraint forces, from Eq.
(4.2.18), are
driT é m i&& fi - m i A i sci ¢f&i 2 ù + dfi é m i sci ¢T A iT PT&&

r + m i PA is ci ¢&& fi ù
ri + J ¢i&&
ë i û ë û
ìr ¢T (q ) é m r ¢(q )q && + m i PA i sci ¢fi¢¢(q , q& ) - m i A is ci ¢ (fi¢(q )q& ) 2 ù ü
&& + m iri ¢¢ (q, q& ) + m i PA is ci ¢fi¢(q )q
ï i
ë i i
ûï
= dq T í ý
ï +fi¢T (q ) é m i s ic¢T A iT P Tri¢(q )&& && + J ¢i fi¢¢(q , q& ) ù
q + m is ci ¢T A Ti P Tri¢¢ (q, q& ) + J ¢i fi¢(q )q ï
î ë û þ
where P = A( p / 2) . Summing the foregoing expression over i and collecting terms,
å {dr
i
i
T é m &&
ë ii
fi - m i A i sic¢f&i 2 ù + dfi é m i sci ¢T A iT PT&&
r + m i PA is ic¢&&
û ë
ri + J i¢&&
û }
fi ù = dq T ( M ( q )q
&& - S(q , q& ) )
(9.10.3)
where
{ ( ) (
M(q) = å ri¢T (q ) m iri¢(q) + m i PA i sci ¢fi¢(q ) + fi¢T (q ) m is ic¢T A iT P Tri¢(q ) + J ¢i fi¢(q )
i
)}
S(q, q& ) = å í
i i (
ì -r ¢T (q) m r ¢¢ (q, q& ) + m PA s c¢f¢¢(q, q& ) - m A sc¢ ( f¢(q )q& )2
ï i i i i i i i i i )ïü (9.10.4)
ý
î i i i i (
i ï - f¢T (q ) m sc ¢T A T PTr ¢¢ (q, q
i
& ) + J ¢i fi¢¢(q, q& ) ) ï
þ
For gravitational force acting in the negative y-direction,
( ) ( )
T
dW q = -å m i gd ri (q ) + A i sci ¢ u y = -å m i g dri¢T (q) + dfi sci ¢T A Ti PT u y
i i
(9.10.5)
(
= -dq å m i g ri¢T (q) + fi¢T (q )sci ¢T A iT PT u y º dq TQq
T
i
)
where the identity dA(f) / df = PA(f) is used. For actuator j, dW = dy j F ,where F is a force
yj yj yj
for a strut and a torque for a motor. Thus,

dW y = å dy j F
yj
(9.10.6)
j
The virtual work dWz of forces that act on the end effector depend on its interaction with the
environment and must be derived on a case-by-case basis.
731
Equations (9.10.3) through (9.10.6), plus end effector virtual work, provide all terms
needed for evaluation of the ODE of Eq. (9.9.12).
For evaluation of terms in Eqs. (9.10.3) through (9.10.5), ri¢(q ) , ri¢¢ (q, q& ) , fi¢(q) , and
fi¢¢(q, q& ) , i = 1,2,3 , must be evaluated. For body 1, s ci¢ = 2u x , J1¢ = 16m1 / 3 , r1 = 0 , and f1 = q1 .
Thus, r1¢(q) = r1¢¢ (q, q& ) = 0 , f1¢(q) = [1 0 0] , and f1¢¢(q, q& ) = 0 . For body 2, s c2¢ = 0 , J¢2 = m2 / 4 ,
r2 = q 2 (cu x + su y ) , and f2 = q1 . Thus, f1¢(q) = [1 0 0] , f1¢¢(q, q& ) = 0 ,
r2¢ (q ) = éëq 2 ( -su x + cu y ) (cu x + su y ) 0 ùû , and
r2¢¢ (q, q& ) = éëq& 1q 2 ( -cu x - su y ) + q& 2 ( -su x + cu y ) q& 1 ( -su x + cu y ) 0 ùû . For body 3,
s c3¢ = 0.25(u x + u y ) , J¢3 = m3 / 2 , r3 = 4A1u x , and f3 = q1 + q3 . Thus, ri¢(q ) = [4PA1u x 0 0] ,
r3¢¢ (q, q& ) = [ -4q& 1 A1u x 0 0] , f3¢(q) = [1 0 1] , and f3¢¢(q, q& ) = 0 , where PP = - I is used. These
terms are used to evaluate functions in Eqs. (9.10.3) through (9.10.5).
All terms required in Eq. (9.9.12) are thus available, so the ODE of motion is
f ¢T (y )M ( f (y ))f ¢(y )&&
y + f ¢T (y )M ( f (y )) W( y , y& ) - f ¢T (y )S( f (y ), f ¢(y )y& )
(9.10.7)
- Q q ( y , y& , t) - F y ( y , t) = 0
The loader does not require the full range of terms possible in M ( f ( y )) , W ( y , y& ) , S(f (y ), f ¢(y )y& ) ,
and Qz ( y , y& , t) = 0 , so Eq. (9.10.7) could be simplified. Since the term Ω q-1 (f (y )) appears often
and can only be evaluated numerically, essentially nothing is gained by simplifying Eq. (9.10.7).
A numerical evaluation of Eq. (9.10.7) is inevitable and simplifying terms for the specific
application does not contribute significantly to computational efficiency.
9.10.2 Stewart Platform
The driving simulator shown in Fig. 9.1.3 with a vehicle cab and graphics system in the
dome was used for off-road applications in the 1990s (Haug, Choi, Kuhl, and Wargo, 1995). The
schematic of Fig. 9.10.1 shows six hydraulic actuators that control position and orientation of the
upper platform, relative to the lower platform. Details of attachment points on the lower platform
of diameter 5 m and the upper platform of diameter 4 m are shown in Fig. 9.10.2. Lower bounds
of y min = 2.7 m and upper bounds of y max = 4.1 m on the lengths of the actuators were selected
by the manufacturer to assure that there are no kinematic singularities in the operational envelope
of the simulator. While this geometry is a slight idealization of the actual simulator, it is adequate
to illustrate performance of the manipulator.
732
Figure 9.10.1 Stewart Platform
Figure 9.10.2 Top and Bottom Platform Geometry

9.10.2.1 Stewart Platform Kinematics
Attachment points in the lower platform, with c º cos(π / 6) and s º sin(π / 6) , are
r A = - 2.5u y , r B =2.5(cu x + su y ) , and r C =2.5( -cu x + su y ) , where ux = [1 0 0] ,
T
u y = [0 1 0] , and uz = [0 0 1] . Attachment points in the upper platform, represented in

T T
the body fixed x ¢-y¢-z¢ frame, are s¢D = 2u y , s¢E = 2( -cu x - su y ) , and s¢F = 2(cu x - su y ) . Euler
T
parameters p = éë e0 e T ùû Î R 4 , with the Euler parameter normalization condition pTp = 1 ,
represent orientation of the upper platform. Euler parameters are used to avoid orientation
transformation singularities that arise if any set of three orientation coordinates is used. The
orthogonal orientation transformation matrix A( p ) of Eq. (2.5.22) transforms vectors
represented in the x ¢-y¢-z¢ frame to a frame that is parallel to the x-y-z frame and has origin at
the origin of the x ¢-y¢-z¢ frame. Thus, vectors that locate points D, E, and F in the x-y-z frame are
r D = r + A(p)s¢D , r E = r + A(p)s¢E , and r F = r + A(p)s¢F , where r is the vector shown in Fig.
9.10.1 from the origin on the x-y-z frame to the origin of the x ¢-y¢-z¢ frame.
Denoting attachment points in the lower platform as α = A, B, or C and those in the
upper platform as β = D, E, or F , the condition that defines the length y α,β of the strut between
points α and β , as a function of r and p, is
733
Ψ α ,β (r , p , y ) = éë(r β - r α )T (r β - r α ) - y α2 ,β ùû / 2
(9.10.8)
= éër Tr + 2r T A (p )s¢β - 2r Tr α - 2s¢βT A T ( p )r α + 10.25 - y α2 ,β ùû / 2 = 0
where AT (p)A(p) = I , s¢βT s¢β = 4 , and r α Tr α = 6.25 are used. Numbering actuators and
associated pairs of lower and upper platform attachment points as in Table 9.10.1, input
equations are
é Ψ1 (r , p, y ) ù é Ψ A,F (r , p, y ) ù
êΨ (r , p, y ) ú ê A,E ú
ê 2 ú ê Ψ (r , p , y ) ú
ê Ψ3 (r , p, y ) ú ê Ψ B,F (r , p, y ) ú
Ψ(r , p, y ) = ê ú = ê B,D ú=0 (9.10.9)
ê Ψ 4 ( r , p , y ) ú ê Ψ ( r , p , y ) ú
êΨ5 (r , p, y ) ú ê ΨC,E (r , p, y ) ú
ê ú ê C,D ú
ë Ψ 6 (r , p , y ) û ë Ψ (r , p , y ) û
Table 9.10.1 Actuator and Input Coordinate Definition
Actuator Lower Att. Pt. Upper Att. Pt. Input
(α) (β) Coord.
1 A F y1 = y A,F
2 A E y2 = yA,E
3 B F y 3 = y B,F
4 B D y 4 = y B,D
5 C E y 5 = yC,E
6 C D y6 = yC,D
The Euler parameter normalization condition pTp = 1 becomes the holonomic constraint
Φ(p ) = ( p Tp - 1) / 2 = 0 (9.10.10)
Combined, Eqs. (9.10.9) and (9.10.10) comprise a system of seven forward kinematic equations,
é Φ(p) ù
Ω(q, y ) = ê ú=0 (9.10.11)
ë Ψ (r , p , y ) û
T
where q = éër T p T ùû .
Derivatives of Φ(p ) and Ψ(r , p, y ) with respect to q are needed to obtain Ωq (q, y ) for
forward kinematic analysis. The simplest result is
Φq (p ) = éë0 p T ùû (9.10.12)
734
Somewhat more intricate is the relation
Yqα,β (q ) = éë Yrα,β (r , p ) Ypα,β (r, p ) ùû = éër T + s¢βT A T (p) - r αT (r T
- r α T ) B(p, s¢β ) ùû (9.10.13)
which does not depend on y and the linear matrix function B(p, s¢β ) of p is defined in Eq.
(2.6.25). With the bookkeeping defined by Eq. (9.10.9) and Table 9.10.1,
ér T + s¢FT A T (p ) - r A T
ê
(r - r AT ) B(p, s¢F ) ù
T
ú
êr T + s¢ET A T (p) - r AT
ê
(rT
- r A T ) B (p, s ¢ E ) ú
ú
ê r T + s¢FT A T (p) - r BT (rT
- r BT ) B(p, s¢F ) ú
Ψq (q ) = ëéΨr (r , p) Ψp (r , p ) ûù = ê ú (9.10.14)
êr T + s¢DT A T (p) - r BT
ê
(rT
- r ) B (p, s ) ú
BT
¢ D
ú
êr T + s¢ET A T (p ) - r CT
ê
(rT
- r CT ) B(p, s¢E ) ú
ú
êr T + s¢DT A T (p) - r CT
ë (rT
- r CT ) B(p, s¢D ) úû
Since F y = 0 , Ψ y (y ) = -diag(y i ) , Ψ -y1 ( y ) = -diag((1/ y i )) , Ωy ( y ) = éë 0 ΨTy ( y )ùû , and

é 0 Φ p (p ) ù
Ωq (q ) = ê ú (9.10.15)
ëΨ r (r , p ) Ψ p (r , p ) û
which does not depend explicitly on y. Finally, with output selected as z = q, Γ( q , z ) = q - z = 0 ,
Γq = I7 , Γq-1 = I7 Γz = -I7 , and Γ -z 1 = -I7 , from Eqs. (9.8.14) and (9.8.15)
f ¢( y ) = -Ωq-1 (f (y ), y )Ω y (y ) and h¢(q ) = I , so q& = f ¢( y )y& and δq = f ¢( y )δy .
The only nonzero second derivatives in the formulation are

F qq (q, a ) = ( Fq (q )aˆ ) = ( pTaˆ p ) = éë 01´3 aTp ùû
q q
T
where a = éëaTr a Tp ùû , ar Î R 3 , ap Î R 4 , and for the components Y α,β
q (r , p , y ) of Ψ q (r , p , y )
defined in Eq. (9.10.14),
qq (q , a ) = ( Y q (q )a ) = ( r + s
Y α,β α,β
ˆ
q
T
{¢βT A T (p ) - r α T ) aˆ r + (r T - r α T ) B(p, s¢β )aˆ p } q
Since r α and s¢β are constant, ( r T + s¢βT A T (p ) - r α T ) aˆ r = aˆ rT (r + A(p )s¢β - r α ) , and

(r T
- r α T ) B(p, s ¢β )aˆ p = aˆ Tp B T (p, s¢β ) ( r - r α ) , so
Y α,β é T ¢β arT B(p, s¢β ) + (r T - r α T ) B(ap , s¢β ) ù

qq (q, y, a ) = ëar + ap B (p, s )
T T
û
where the identity B(p, s¢β )aˆ p = B(aˆ p , s¢β )p of Eq. (2.6.28) has been used. These easily
implementable results yield the only second derivative matrix that appears in Eq. (9.9.9),
735
éF qq ( q, a ) ù
Ω qq (q , a ) = ê ú (9.10.16)
ëΨ qq ( q, a ) û
Thus, Eq. (9.9.9) reduces to

W( y , y& ) = Wq-1 (f ( y ))Wqq ( f ( y )b )f ¢(y )y& (9.10.17)
where b = Ω q-1 (q )Ω y (y )y& .
9.10.2.2 Stewart Platform Equations of Motion

The variational equation of motion of Eq. (4.6.17) for the single body that comprises the
Stewart platform mechanism, in noncentroidal x ¢-y¢-z¢ coordinates, is
{
dq T M(q)q
&& - S(q,q)
& - Q q (q,q) }
& - δy T F y (y, t ) = 0 (9.10.18)
where
é mI -2mAs% ¢cG ù
M(q) = ê ú
ë2mG s%¢ A 4GT J ¢G û
T c T
é 0 ù
& =ê T
S(q,q) ú (9.10.19)
& &
ë8G J ¢Gp û
é F ù
& =ê T ú
Qq (q,q)
ë2G n¢û
and s¢c = 0.5u z is the vector from the origin of the x ¢-y¢-z¢ frame to the centroid of the platform,
m is platform mas, J ¢ is the inertia matrix of the platform relative to the x ¢-y¢-z¢ frame, and
E = E(p ), G = G(p ), and A = A(p ) are defined in Section 2.6. The more massive component of
each hydraulic strut is attached to ground and has little influence on motion of the platform. The
mass of the lighter component of each strut that is attached to the massive platform is lumped
with the platform at its attachment point. There are no generalized force terms associated with
the output coordinates, F = -mgu z is the gravitational force acting at the origin of the body-fixed
x ¢-y¢-z¢ frame, and n ¢ = - mgs% ¢c u y is the resultant external torque acting on the platform about
the origin of the x ¢-y¢-z¢ frame, represented in that frame. Finally, the compressive forces
Fyi ( y,t) in hydraulic struts i, i = 1, … ,6, control motion of the platform.
The differential equation of motion of the platform is evaluated from Eq. (9.9.12) using
the foregoing quantities, as
f ¢T (y )M ( f (y ))f ¢(y )&&
y + f ¢T (y )M ( f (y )) W( y , y& ) - f ¢T (y )S( f (y ), f ¢(y )y& )
(9.10.20)
- Q q ( y , y& , t) - F y ( y , t) = 0
736
As in the case of the front-end loader, these equations can be simplified, but they depend
on the inverse matrix Ω q-1 (f (y )) that appears often and must be evaluated numerically. Little is
gained, therefore, in carrying out the simplification.
Direct application of the ODE of dynamics derived in Section 9.9, with the equations of motion of
planar bodies in Section 4.2, yields equations of motion for the front-end loader, without any ad-hoc
derivation. Similarly, direct application of the ODE of dynamics, with the equations of motion of
spatial bodies in Section 4.3 yields equations of motion for the Stewart platform, without any ad-hoc
derivation.
737
Appendix 9.A Key Formulas, Chapter 9
738
Bibliography
Abraham, R, and Marsden, J.E., 1978, Foundations of Mechanics, Second Ed.,
Benjamin/Cummings, Reading, Mass.
Adkins, F. A., and Haug, E. J., 1997, Operational Envelope of a Spatial Stewart Platform,
Journal of Mechanical Design, Vol. 119, pp. 330-332.
Anderson, K.S., and Hsu, Y., 2004, Order-(n+m) direct differentiation determination of design
sensitivity for constrained multibody dynamic systems, Struct Multidisc Optim, vol. 26, pp 171-
182.
Arnold, M., 2017, DAE aspects of multibody system dynamics, In: A. Ilchmann, T. Reis (eds.):
Surveys in Differential-Algebraic Equations IV, Differential-Algebraic Equations Forum, pp.
41-106. Springer International Publishing.
Arnold, M., and Bruls, O., 2007, Convergence of the Generalized- a Scheme for Constrained
Mechanical Systems, Multibody System Dynamics, Vol.18, No.2, pp.185-202.
Arnold, V. I., 1978, Mathematical Methods of Classical Mechanics, Springer, New York.
Ascher, U.M., and Petzold, L.R., 1998, Computer Methods for Ordinary Differential Equations
and Differential-Algebraic Equations, SIAM, Philadelphia.
Atkinson, K.E.,1989, An Introduction to Numerical Analysis, Second Ed., Wiley, New York.
Banerjee, J., and McPhee, J, 2016, Graph-theoretic sensitivity analysis of multi-domain dynamic
systems: theory and symbolic computer implementation, Nonlinear Dynamics, vol. 85, pp. 203-
227.
Banerjee, J., and McPhee, J, 2016, Graph-theoretic sensitivity analysis of multi-domain dynamic
systems: theory and symbolic computer implementation, Nonlinear Dynamics, vol. 85, pp. 203-
227.
Bauchau, O.A., and Laulusa, A., 2008, Review of Contemporary Approaches for Constraint
Enforcement in Multibody Systems, Journal of Computational and Nonlinear Dynamics, Vol. 3,
p. 0110005.
Berger, E.J., 2002, Friction modeling for dynamic system simulation, Applied Mechanics
Reviews, Vol. 55, No. 6, pp535-577.
Bernstein, D.S., 2009, Matrix Mathematics, Second Ed., Princeton University Press, Princeton,
NJ.
Betsch, P., 2004, A Unified Approach to the Energy-Consistent Numerical Integration of
Nonholonomic Mechanical Systems and Flexible Multibody Dynamics, GAMM-Mitt., Vol. 27,
No. 1, p. 66.
Bianchi, G., and Schiehlen, W. (eds.), 1986, Dynamics of Multibody Systems, Springer-Verlag,
Berlin.
Bloch, A.M., 2003, Nonholonomic Mechanics and Control, Springer, New York.
Blumentals, A., Brogliato, B, and Bertails-Descoubes, F., 2016, The contact problem in
Lagrangian systems subject to bilateral and unilateral constraints, with or without sliding
Coulomb’s friction: a tutorial, Multibody System Dynamics, Vol. 38, pp.43-76.
739
Bohigas, O., Montserrat, M., and Ros, L, 2017, Singularities of Robot Mechanisms, Springer
International Publishing, Switzerland.
Borri, M., Bottasso, C., and Mantegazza, P., 1990, Equivalence of Kane’s and Maggi’s
Equations, Meccanica, Vol. 25, No. 4, pp. 272-274.
Borri, M., Bottasso, C., and Mantegazza, P., 1992, Acceleration Projection Method in Multibody
Dynamics, European Journal of Mechanics, A/Solids, Vol. 11, No. 3.
Brenan, K.E., Campbell, S.L., and Petzold, L.R., 1989, Numerical Solution of Initial-Value
Problems in Differential-Algebraic Equations, North Holland, New York.
Brown, P., and McPhee, J., 2016, A Continuous Velocity-Based Friction Model for Dynamics
and Control with Physically Meaningful Parameters, Journal of Computational and Nonlinear
Dynamics, Vol. 11.
Bruls, O., and Cardona, A., 2010, On the Use of Lie Group Time Integrators in Multibody
Dynamics, Journal of Computational and Nonlinear Dynamics Vol.5, No.3, pp.1-13.
Bruls, O., Cardona, A., and Arnold, M., 2012, Lie group generalized- a time integration of
constrained flexible multibody systems, Mechanism and Machine Theory, Vol. 48, pp.121-137.
Bryson, A.E., and Ho, Y.C., 1975, Applied Optimal Control, Wiley, NY.
Callejo, A., and Garcıa de Jalon, J., 2014, A Hybrid Direct-Automatic Differentiation Method for
the Computation of Independent Sensitivities in Multibody Systems, Int. J. Numer. Meth.
Engrg., vol. 100, pp.933-952.
Chablat, D., and Wenger, P., 1998, Working modes and aspects in fully parallel manipulators,
Proceedings of the 1998 IEEE International Conference on Robotics and Automation, Leuven,
Belgium, pp. 1964-1969.
Chung, J., and Hulbert, G.M., 1993, A Time Integration Algorithm for Structural Dynamics with
Improved Numerical Dissipation: The Generalized- α Method, Journal of Applied Mechanics,
Vol. 60, pp. 371-374.
Coddington, E.A., and Levinson, N., 1955, Theory of Ordinary Differential Equations, McGraw-
Hill, New York.
Conconi, M., and Carricato, M., 2009, A New Assessment of Singularities of Parallel Kinematic
Chains, IEEE Transactions on Robotics, Vol. 25, No. 4, pp. 757-770.
Corwin, L.J., and Szczarba, R.H., 1982, Multivariable Calculus, Marcel Dekker, New York.
Coste, M., 2012, A Simple Proof That Generic 3-RPR Manipulators Have Two Aspects, J. of
Mechanisms and Robotics, Vol. 4, pp. 01108, 1-6.
Coste, M., Wegner, P., and Chablat, D., 2018, Hidden Cusps, Advances in Robot Kinematic
2016, Lenarcic and Merlet (eds.), pp. 129-138, Springer Proceedings in Advanced Robotics ,
Springer International Publishing AG, Cham, Switzerland.
d'Alembert, J. le R., 1743, Traite de dynamique, Paris.
Dontchev, A.L. and Rockafellar, R.T., 2014, Implicit Functions and Solution Mappings, Second
Ed., Springer, New York.
740
Dopico, D., Zhu, Y., Sandu, A., and Sandu, C., 2015, Direct and adjoint sensitivity analysis of
Ordinary differential equation multibody formulations, Journal of Computational and Nonlinear
Dynamics, vol. 10, pp. 011012-1-7.
Dundas, B.I., 2018, A Short Course in Differential Topology, Cambridge University Press,
Cambridge, UK.
Eich-Soellner, E., and Fuhrer, K., 1998, Numerical Methods in Multibody Dynamics, B. G.
Teubner, Stuttgart.
Euler, L., 1736, Mechanica, sive motus scisntia analytice exposita, St. Petersburg.
Featherstone, R., 2008, Rigid Body Dynamics Algorithms, Springer, New York.
Featherstone, R., and Orin, D.E., 2016, Dynamics, Springer Handbook of Robotics, Second Ed.
Springer-Verlag, Berlin.
Frank, P.M.,1978, Introduction to System Sensitivity Theory, Academic Press, NY.
Garcia de Jalon, J., Callejo, A., and Hidalgo, A.F., 2012, Efficient Solution of Maggi’s
Equations, J. of Computational and Nonlinear Dynamics, Vol 7, p. 021003.
Garcia-Naranjo, L.C., and Marrero, J.C., 2013, Non-Existence of an Invariant Measure for a
Homogeneous Ellipsoid Rolling on the Plane, Regular and Chaotic Dynamics, Vol. 18, No.4, p.
372.
Gear, C.W., and Petzold, L.R., 1984, ODE Methods for the Solution of Differential/Algebraic
Systems, SIAM Journal of Numerical Analysis, Vol. 21, No. 4, pp. 716-728.
German, H.C., 2006, Computer-Aided Design Sensitivity Analysis for Dynamic Multibody
Systems, Ph.D. Thesis, Department of Mechanical Engineering, The University of Iowa.
Ghosal, A, and Ravani, B., 2001, A Differential-Geometric Analysis of Singularities of Point
Trajectories of Serial and Parallel Manipulators, Journal of Mechanical Design, Vol. 123, pp. 80-
89.
Goldstein, H., 1980, Classical Mechanics, Second Ed., Addison-Wesley, Reading, Mass.
Gosselin, C., and Angeles, J., 1990, Singularity Analysis of Closed-Loop Kinematic Chains,
IEEE Transactions on Robotics and Automation, Vol. 6, No. 3, pp. 281-290.
Guillemin, V. and Pollack, A., 1974, Differential Topology, AMS Chelsea Publishing,
Providence, RI.
Gutierrez-Lopez, M.D., Callejo, A., Garcıa de Jalon, J., 2012, Computation of Independent
Sensitivities Using Maggi’s Formulation, 2nd Joint International Conference on Multibody
System Dynamics, Stuttgart.
Hadamard, J., 1902, Sur les problèmes aux dérivées partielles et leur signification
physique. Princeton University Bulletin. pp. 49–52.
Hairer, E., and Wanner, G., 1996, Solving Ordinary Differential Equations II: Stiff and
Differential-Algebraic Problems, Second Ed., Springer-Verlag, Berlin.
Hairer, E., Lubich, C., and Wanner, G., 2006, Geometric Numerical Integration, Second Ed.,
741
Hairer, E., Norsett, S.P., and Wanner, G., 1993, Solving Ordinary Differential Equations I:
Nonstiff Problems, Second Ed., Springer-Verlag, Berlin.
Hart, W.L., 1957, Analytical Geometry and Calculus, Heath, Boston.
Haug, E.J., 1984, Computer Aided Analysis and Optimization of Mechanical System Dynamics,
Haug, E.J., 1987, Design Sensitivity Analysis of Dynamic Systems, Mota Soares, C.A. (ed.),
Computer Aided Optimal Design: Structural and Mechanical Systems, Springer, Berlin, pp. 705–
755.
Haug, E.J., 1989, Computer-Aided Kinematics and Dynamics of Mechanical Systems, Vol. I:
Basic Methods, Allyn and Bacon, Boston.
Haug, E.J., 1992, Intermediate Dynamics, Prentice-Hall, Englewood Cliffs, NJ.
Haug, E.J., and Yen, J., 1992, Implicit Numerical Integration of Constrained Equations of
Motion Via Generalized Coordinate Partitioning, Journal of Mechanical Design, Vol. 114, pp.
296-304.
Haug, E.J., 2016, An Ordinary Differential Equation Formulation for Multibody Dynamics:
Holonomic Constraints, Journal of Computing and Information Science in Engineering, Vol. 16,
No. 2, p. 021007.
Haug, E.J., 2017a, An Ordinary Differential Equation Formulation for Multibody Dynamics:
Nonholonomic Constraints, Journal of Computing and Information Science in Engineering, Vol.
17, No. 1, p 0110009.
Haug, E.J., 2017b, An Index 0 Differential-Algebraic Equation Formulation for Multibody
Dynamics-Holonomic Constraints, Mechanics Based Design of Structures and Machines, Vol.
45, No. 4, pp. 479-506.
Haug, E.J., 2018a, An Index 0 Differential-Algebraic Equation Formulation for Multibody
Dynamics-Nonholonomic Constraints, Mechanics Based Design of Structures and Machines,
Vol. 46, No. 1, pp. 38-65.
Haug, E.J., 2018b, Simulation of Friction and Stiction in Multibody Dynamics Model Problems,
Mechanics Based Design of Structures and Machines, Vol. 46, No. 3, pp. 296-317.
Haug, E.J., 2018c, Simulation of Spatial Multibody Systems with Friction, Mechanics Based
Design of Structures and Machines, Vol. 46, No.3, pp.347-375.
Haug, E.J., 2018d, Extension of Maggi and Kane Equations to Holonomic Dynamic Systems,
Journal of Computational and Nonlinear Dynamics, Vol. 13, pp.121003.
Haug, E.J., 2020a, Well-posed formulation of holonomic mechanical dynamics and design
sensitivity analysis, Mechanics Based Design of Structures and Machines, Vol 48, No 1, pp. 111-
121.
Haug, E.J., 2020b, Parallel Manipulator Domains of Singularity Free Functionality, Mechanics
Based Design of Structures and Machines, doi.org/10.1080/15397734.2020.1728547.
Haug, E.J., 2020c, Well-Posed Formulations for Nonholonomic Mechanical System Dynamics,
Journal of Computational and Nonlinear Dynamics, Vol. 15, p. 101003.
742
Haug, E.J., 2020d, Parallel Manipulator Dynamics Embedded in Singularity Free Domains of
Functionality, Mechanics Based Design of Structures and Machines,
doi.org/10.1080/15397734.2020.1776132.
Haug, E.J., and Arora, J.S., 1979, Applied Optimal Design, Wiley, NY.
Haug, E. J., Choi, K. K., Kuhl, J. K., and Wargo, J. D., "Virtual Prototyping Simulation for
Design of Mechanical Systems," Journal of Mechanical Design, Vol. 117, 1995, pp. 63-70.
Haug, E.J., Luh, C.M., Adkins, F.A., and Wang, J., 1996, Numerical Algorithms for Mapping
Boundaries of Manipulator Workspaces, Journal of Mechanical Design, Vol. 118, pp. 228-234.
Haug, E.J., Mani, N.K., and Krishnaswami, P., 1984, Design Sensitivity Analysis and
Optimization of Dynamically Driven Systems, Computer Aided Analysis and 0ptimization of
Mechanical System Dynamics, E.J. Haug (Ed.), Springer-Verlag, Heidelberg, pp. 555-636.
Haug, E. J., Wang, J. T., and Wu, J. K., 1992, Dexterous Workspaces of Manipulators, Part I:
Analytical Criteria, Mechanics of Structures and Machines, Vol. 20, No. 3, pp. 321-361
Haug, E.J., Wu, S.M., and Yang, S.M., 1986, Dynamics of Mechanical Systems with Coulomb
Friction, Stiction, Impact, and Constraint Addition and Deletion-I Theory, Mechanism and
Machine Theory, Vol. 21, No. 5, pp. 401-406.
Hilber, H.M., Hughes, T.J.R., and Taylor, R.L., 1977, Improved Numerical Dissipation for Time
Integration Algorithms in Structural Dynamics, Earthquake Engineering and Structural
Dynamics, Vol. 5, No. 3, pp. 283-292.
Hopf, H., 1940, Systeme symmetrischer Bilinearformen und euklidsche Modelle der projektiven
Raume, Vierteljschr. Naturforsch. Ges. Zurich.
Husty, M.L., 2009, Non-singular assembly mode change in 3-RPR-parallel manipulators,
Computational Kinematics, Springer-Verlag, pp. 51-60.
Innocenti, C., and Parenti-Castelli, V. 1998, Singularity-Free Evolution From One Configuration
to Another in Serial and Fully-Parallel Manipulators, Journal of Mechanical Design, Vol. 120,
No. 1, pp. 73-79.
Kane, T.R., and Levinson, D.A., 1985, Dynamics: Theory and Applications, McGraw-Hill, New
York.
Kato, T., 1980, Perturbation Theory for Linear Operators, Second Ed., Springer Verlag, New
York.
Kelley, C.T., 1995, Iterative Methods for Linear and Nonlinear Equations, SIAM, Philadelphia.
Kreyszig, E., 2011, Advanced Engineering Mathematics, Eighth Ed., Wiley, New York.
Lagrange, J.L., 1788, Mechanique Analytique, Paris.
Lanczos, C., 1962, The Variational Principles of Mechanics, Second Ed., University of Toronto
Press, Toronto.
Laulusa, A., and Bauchau, O.A., 2008, Review of Classical Approaches for Constraint
Enforcement in Multibody Systems, Journal of Computational and Nonlinear Dynamics, Vol. 3,
p. 011004.
Lee, J.M., 2010, Introduction to Topological Manifolds, Second Ed., Springer, New York.
743
Lee, J.M., 2013, Introduction to Smooth Manifolds, Second Ed., Springer, New York.
Li, H., Gosselin, C.M., and Richard, M.J., 2006, Determination of maximal singularity-free
zones in the workspace of planar three-degree-of-freedom parallel mechanisms, Mechanism and
Machine Theory, Vol. 41, pp. 1157-1167.
Liang, C.G., and Lance, G.M., 1987, A Differentiable Null Space Method for Constrained
Dynamic Analysis, Journal of Mechanisms, Transmissions and Automation in Design, Vol. 109,
pp. 405-411.
Lovelock, D., and Rund, H., 1975, Tensors, Differential Forms, and Variational Principles,
Wiley, New York.
Luh, C.M., Adkins, F.A., Haug, E.J., and Qiu, C.C., 1996, Working Capability Analysis of
Stewart Platforms, Journal of Mechanical Design, Vol. 118, pp. 220-227.
Lynch, K.M., and Park, F.C., 2017, Modern Robotics, Cambridge University Press, Cambridge,
UK.
Maggi, G.A., 1896, Principii della Teoria Matematica del Movimento dei Corpi: Corso de
Meccanica Razionale, Ulrico Hoepli, Milano.
Maggi, G.A., 1901, Di Alcune Nuove Forme delle Equazioni della Dinamica Applicabili ai
Sistemi Anolonomi, Rendiconti della Regia Academia dei Lincei, Serie V, Vol. X, pp. 287-291.
Magnus, K. (ed.), 1978, Dynamics of Multibody Systems, Springer-Verlag, Berlin.
(Symposium Munich/Germany August 29–September 3, 1977)
Marques, F., Flores, P., Pimenta Claro, J.C., and Lankarani, H.M., 2016, A survey and
comparison of several friction force models for dynamic analysis of multibody mechanical
systems, Nonlinear Dynamics, Vol. 86, No. 3, pp 1407-1443.
Mendelson, B., 1962, Introduction to Topology, Allyn and Bacon, Boston.
Merlet, J.-P., 2006, Parallel Robots, Second Ed., Springer, The Netherlands.
Merlet, J.-P., Gosselin, C., and Huang, T., 2016, Parallel Mechanisms, Springer Handbook of
Robotics, Second Ed. Springer-Verlag, Berlin.
Muller, A. and Terze, Z, 2016, Geometric methods and formulations in computational multibody
system dynamics, Acta Mech, Vol 227, pp. 3327-3350.
Murray, M., Li, Z., and Sastry, S.S., 1994, A Mathematical Introduction to Robotic
Manipulation, CRC Press, Boca Raton, FL.
Negrut, D, Jay, L.O., and Khude, N., 2009, A Discussion of Low-Order Numerical Integration
Formulas for Rigid and Flexible Multibody Dynamics, Journal of Computational and Nonlinear
Dynamics, Vol. 4, pp. 121008 1.
Negrut, D., Haug, E.J., and German, H.C., 2003, An Implicit Runge-Kutta Method for
Integration of Differential-Algebraic Equations of Multibody Dynamics, Multibody System
Dynamics, Vol 9, pp 121-142.
Negrut, D., Rampalli, R., Ottersson, G., and Sajdak, A., 2007, On an Implementation of the
Hilber-Hughes-Taylor Method in the Context of Index 3 Differential-Algebraic Equations of
Multibody Dynamics, Journal of Computational and Nonlinear Dynamics, Vol. 2, pp. 73-85.
744
Neimark, J.I., and Fufaev, N.A.,1972, Dynamics of Nonholonomic Systems, American
Mathematical Society, Providence, Rhode Island.
Newmark, N.M., 1959, A Method of Computation for Structural Dynamics, Journal of the
Engineering Mechanics Division, ASCE, Vol. 85, No. EM3, pp. 67-94.
Newton, I., 1687, Philosophiae naturalis principia mathematica, London.
Orlandea, N., Chace, M.A., and Calahan, D.A., 1977, A Sparsity-Oriented Approach to the
Dynamic Analysis and Design of Mechanical Systems, Parts I and II, Journal of Engineering for
Industry, Vol. 99, pp. 773-784.
Palais, B., Palais, R., and Rodi, S., 2009, A Disorienting Look at Euler’s Theorem on the Axis of
Rotation, American Mathematical Monthly, Vol. 116, No. 10, pp. 892-909.
Papastavridis, J.G., 1990, The Maggi or Canonical Form of Lagrange’s Equations of Motion of
Holonomic Mechanical Systems, J. of Applied Mechanics, Vol. 57, pp. 1004-1010.
Pars, L.A., 1965, A Treatise on Analytical Dynamics, Reprint by Ox Bow Press (1979),
Woodbridge, Conn.
Peidro, A., Marin, J.M., Gil, A., and Reinoso, O., 2015, Performing nonsingular Transitions
Between Assembly Modes in Analytical Parallel Manipulators by Enclosing Quadruple
Solutions, J. of Mechanical Design, Vol. 137, p.122302.
Pennestri,E., Rossi, V., Salvini, P., and Valentini, PP., 2016, Review and Comparison of dry
friction force models, Nonlinear Dynamics, Vol.83, pp. 1785-1801.
Petzold, L.D., 1982, Differential/Algebraic Equations are not ODE’s, SIAM Journal of Scientific
and Statistical Computing, Vol. 3, No. 3, pp. 367-384.
Petzold, L.R., 1986, Order Results for Implicit Runge-Kutta Methods Applied to
Differential/Algebraic Systems, Vol. 23, No.4, pp. 837-853.
Potra, F.A., and Rheinboldt, W.C., 1991, On the Numerical Solution of the Euler-Lagrange
Equations, Mechanics of Structures and Machines, Vol. 19, No. 1, pp. 1-18.
Qiu, C. C., Luh, C. M., and Haug, E. J., 1995, Dexterous Workspaces of Manipulators, Part III:
Calculation of Continuation Curves at Bifurcation Point, Mechanics of Structures and Machines,
Vol. 23, No. 1, pp. 115-130.
Rabier, P.J., and Rheinboldt, W.C., 2000, Nonholonomic Motion of Rigid Mechanical Systems
from a DAE Viewpoint, SIAM, Philadelphia.
Rabier, P.J., and Rheinboldt, W.C., 2002, Theoretical and Numerical Analysis of Differential-
Algebraic Equations, Handbook of Numerical Analysis, Vol VIII (ed. P.G. Ciarlet and J.L.
Lions), Elsevier Science B.V., Amsterdam, pp. 183-540.
Rheinboldt, W.C., 1991, On the Existence and Uniqueness of Solutions of Nonlinear Semi-
Implicit Differential-Algebraic Equations, Numerical Analysis, Theory, Methods, &
Applications, Vol 16, No. 7/8, pp. 647-661.
Saha, S.K., and Angeles, J., 1991, Dynamics of Nonholonomic Mechanical Systems Using a
Natural Orthogonal Complement, ASME Journal of Applied Mechanics, Vol. 58, pp.238-243.
Schiehlen, W. (ed.), 1990, Multibody Systems Handbook, Springer-Verlag, Berlin.
745
Schiehlen, W. (ed.), 1993, Advanced Multibody System Dynamics: Simulation and Software
Tools, Kluwer Academic Publishers, Dordrecht.
Schiehlen, W., Eismann, W., 1994, Reduction of Nonholonomic Systems, Collection of Papers
Dedicated to Professor P. S. Theocaris, Kounadis, A.N. (ed.), National Technical University of
Athens, pp. 207 - 220.
Schlichtkrull, H., 2015, Differentiable Manifolds, Department of Mathematical Sciences,
University of Copenhagen, Denmark, ISBN 978-87-7078-568-6, available at
http://web.math.ku.dk/noter/filer/geom2-2015.pdf.
Schweizer, B., and Pu, L., 2016, Solving Differential-Algebraic Equation Systems: Alternative
Index-2 and Index-1 Approaches for Constrained Mechanical Systems, Journal of Computational
and Nonlinear Dynamics, Vol 11, p. 044501.
Sefrioui, J. and Gosselin, C., 1995, On the Quadratic Nature of the Singularity Curves of Planar
Three-Degree-of-Freedom Parallel Manipulators, Mech. Mach Theory, Vol. 30, No.4, pp. 533-
551.
Serban, R., and Haug, E.J., 1998, Kinematic and Kinetic Derivatives in Multibody System
Analysis, Mechanics of Structures and Machines, Vol. 26, No. 2, pp.145-173.
Sneddon, I.N., 1957, Elements of Partial Differential Equations, McGraw-Hill, New York.
Spivak, M, 1999, A Comprehensive Introduction to Differential Geometry, Volume One, Third
ed., first published 1970, Publish or Perish, Houston.
Strang, G., 1980, Liner Algebra and its Applications, Second Ed., Academic Press, New York.
Stuelpnagel, J., 1964, On the Parametrization of the Three-Dimensional Rotation Group, SIAM
Review, Vol. 6, No. 4, pp. 422-430.
Terze, Z., Muller, A. and Zlatar, D., 2015, Lie-group integration method for constrained
multibody systems in state space, Multibody System Dynamics, Vol.34, No.3, pp.275-305.
Teschl, G, 2012, Ordinary Differential Equations and Dynamical Systems, American Math
Society, Providence, Rhode Island.
Tomovic, R., and Vukobratonic, M., 1972, General Sensitivity Theory, American Elsevier, NY.
Tseng, F.-C., Ma, Z.-D., and Hulbert, G.M., 2003, Efficient numerical solution of constrained
multibody dynamics systems, Computer methods in applied mechanics and engineering, Vol.
192, pp 439-472.
Uicker, J.J., Ravani,B., and Sheth, P.N., 2013, Matrix Methods in the Design Analysis of
Mechanisms and Multibody Systems, Cambridge University Press, New York.
Wang, X., 2007, Design Sensitivity Analysis 0f Rigid and Flexible Multibody Systems, Ph.D.
Thesis, Department of Mechanical Engineering, The University of Iowa.
Wang, X., Haug, E.J., and Pan, W., 2005, Implicit Numerical Integration for Design Sensitivity
Analysis of Rigid Multibody Systems, Mechanics Based Design of Structures and Machines, vol.
33, no. 1, pp. 1–30.
746
Wehage, R. A., and Haug, E. J., 1982, Generalized Coordinate Partitioning for Dimension
Reduction in Analysis of Constrained Dynamic Systems, Journal of Mechanical Design, Vol.
104, No. 1, pp. 247-255.
Wittenburg, J., 1977, Dynamics of Systems of Rigid Bodies, B.G. Teubner, Stuttgart.
Wojtyra, M, 2016, Modeling of static friction in closed-loop kinematic chains-Uniqueness and
parametric sensitivity problems, Multibody System Dynamics, Vol. 39, No.4, pp. 337-361.
747
Index
A assembled configuration, centroidal reference frame,

194–195 256, 261, 265, 267, 282
A-stable, 302
assembly mode, 692, 702 chain rule of differentiation,
absolute error tolerance, 301
atlas, 171, 176, 182 40, 118, 366, 509
abstract definition of
Atol, 445, 451 chart, 171, 394, 638, 689
manifold, 168
ATran, 409, 438, 570, 580 closed kinematic loop, 9, 681
acceleration, 17, 31
automobile suspension, 5, 155 closed set, 42
acceleration analysis, 196
axis of symmetry, 266 coefficient of dynamic
acceleration constraint, 11
friction, 593
action and reaction, 246
B coefficient of static friction,
actuator, 276, 678
593
actuator force, 276 backward differentiation, 298
coil spring, 2, 422
Add function, 409, 570, 580 ball and socket joint, 139
collinear, 67, 242, 252
admissible displacement, ball joint, 5
column matrix, 25, 33
244–245, 498 balls out, 4
column vector, 25, 33
admissible ranges of stiction bearing, 4
compatible charts, 182
force, 606 bearing load, 428
compliant component, 2
aerodynamic drag, 351 BEval, 409
components of a vector, 22,
algebraic constraint equations, bifurcation, 15
60, 176, 181
117 bifurcation point, 190
composite joint, 144
algebraic representation of a body fixed reference frame,
computational kinematics,
geometric vector, 24 50, 58
198
algebraic vector, 17, 25 building block constraints,
computer-aided design, 274
algebraic vector analysis, 19 136–137, 625
computer-aided engineering,
algorithmic damping, 573 Butcher tableau, 299
274
alternative DAE formulation,
condition number, 15, 17, 47,
553 C
174, 344, 355, 363, 372,
analytical dynamics, 15, 243
c-space, 684 398, 420, 515, 535, 537,
angular velocity, 4, 70–71,
Cartesian components of a 597, 638, 664
258
vector, 21–22 configuration degrees of
angular velocity form of
Cartesian coordinates, 14 freedom, 513
equations of motion, 258
Cartesian generalized configuration generalized
AppData function, 198, 215,
coordinates, 125, 136, 263, coordinates, 499
406, 412, 418, 422, 425,
377 configuration space, 117,
427, 429, 434, 441, 443,
Cartesian reference frame, 21 165, 168
448, 451, 453, 567, 575,
center of mass, 260–261 configuration tangent space,
583, 662, 665, 668, 671
centrifugal forces, 4 498
applied force, 246, 328
centripetal acceleration, 31 connected set, 181
articulated vehicle, 516, 538
centroid, 254, 260–261, 265 connecting rod, 3
aspect, 686
748
conservation of energy, 350 DAE, 12, 241, 329, 552 dot2 constraint, 137, 141
conservative system, 318, damping coefficient, 276, dot product, 22
345, 351, 420, 518, 521 429, 431 double pendulum, 130, 412,
constrained equations of deformable component, 2 572, 585
motion, 329 degrees of freedom, 3, 171, driving constraint, 193
constraint addition-deletion, 176, 285, 682–683 driving simulator, 678
608 dependent generalized dumbbell, 248
constraint drift, 358, 501 coordinates, 12
constraint error, 649, 651 dependent variables, 491 E
constraint error control, 373, derivative of a matrix, 39
earth-fixed reference frame,
375, 389, 400, 529 derivative operator, 17
242
constraint force, 620 design of bearings, 4, 321
effective coefficient of
constraint geometry, 613 design sensitivity analysis,
friction, 600
constraint Jacobian, 11, 262
ellipsoid, 522
118–119, 186, 244, 285, design variables, 256, 296
embedded equations of
353 design variation, 190, 256
manipulator dynamics, 16
constraint manifold, 168, diagonal matrix, 34
end effector, 7, 727
245, 364, 394, 638 differentiable manifold, 176,
energy dissipation, 2
constraint reaction force, 182, 682, 689–690
energy input, 2
154, 241, 246–247, 328, differential, 57, 72, 244, 348
equal matrices, 34
361, 391, 590, 614, 618, differential constraint, 45,
equal vectors, 25
624 157–158, 162, 497, 504,
equations of motion with
constraint reaction force 520, 542
friction, 636
redundancy, 613 differential form, 45, 244
error control, 308
contact force, 625 differential geometry, 15–16,
error tolerance, 356, 408, 436
contact geometry, 613 171–172, 175, 682, 691
estimate of solution error, 301
continuation of integration, 15 differential mass, 252, 258
Euclidean space, 17, 40, 180
continuous friction model, differential topology, 171,
Euler angle singularity, 104
593, 600, 608, 611, 627, 682
Euler angles, 18, 101
663 differential-algebraic
Euler parameter identities, 87
continuously differentiable equations, 12, 241, 264,
Euler parameter
basis, 357 312, 327, 329, 552
normalization condition,
control of motion, 676 differentiation index, 531, 552
79, 119, 151, 193, 624
coordinate vectors, 60 differentiation rules, 69
Euler parameters, 78–79,
Coriolis force, 328 directed line segment, 19
345, 523
Coulomb friction, 590 direction cosine, 22, 61
Euler's theorem, 76
Coulomb friction force, 610 direction cosine matrix, 52,
exact differential form, 45
Coulomb friction inequalities, 61
explicit integration methods,
590, 602 discrete time grid, 298
298, 300, 393, 505, 533,
coupler, 5 disk rolling without slip, 160
561, 595, 636
crank, 5 distance between vectors, 41
explicit Nystrom4 integration
cross product, 23 distance constraint, 118, 125,
algorithm, 300, 598, 664
cylindrical joint, 142, 445, 137, 199, 215, 252, 258,
external force, 252, 255, 258
624, 627 645
external torque, 255, 349
distance driver, 129, 149
D domains of functionality,
F
181, 676, 682, 691, 694,
d'Alembert's principle, 11,
706, 713 feedback control, 2
241–242, 246, 327
dot1 constraint, 137, 140
749
first form of the fundamental 727 612
equation of motion, 247 generalized form of second independent generalized
first order initial-value order ODE, 312 coordinates, 3, 12, 15, 287,
problem, 296 generalized friction force, 622 340, 491
first order kinematic ODE, geometric nonlinearity, 2 independent velocity
504 geometric vector, 17, 19 coordinates, 501
first order ODE of motion, geometry of joints, 624 Index 0 DAE, 391–392, 404,
504 global reference frame, 50, 433, 531, 612, 614, 641,
first order ordinary 118 658, 664
differential equation, 298 gravitational force, 242, 255 Index 0 DAE initial-value
first order SDIRK formulas, gravity, 2 problem, 533
310 ground body, 118 Index 1 DAE, 555, 573, 638
fly-ball governor, 4 Index 2 DAE, 556, 573
force of constraint, 11, 242, H Index 3 DAE, 16, 552, 556,
246, 624 574, 599, 645, 664
Hausdorff, 42, 181
forward kinematic mapping, index of DAE, 552
HHT, 312, 558, 561, 563,
687 induced topology, 42, 181
566, 572, 575, 579, 583,
forward kinematics, 722–723 inertia matrix, 260, 265, 267
585
forward manipulator inertia tensor, 260
hidden constraints, 552, 554
Jacobian, 724 inertial reference frame, 11,
highly oscillatory, 417
forward velocity mapping, 242, 258
holonomic constraint,
724 infinitesimal displacement, 73
117–118, 162, 193, 243,
four-bar linkage, 5, 450 infinitesimal variation,
281, 353, 391, 497, 516,
frame of reference, 242 72–73, 253, 259
520, 538, 541, 551, 683,
free length, 276, 452 initial conditions, 243, 285
721
friction force, 2, 246, 391, initial time, 285
holonomic stiction constraint,
590, 619, 621, 624, 641, initial velocity conditions,
608
650, 658, 662, 664, 667 286
holonomic velocity constraint,
friction force function, 593 initial-value problem, 243,
244
friction torque, 662 298, 505, 515
homeomorphism, 171, 175,
front-end loader, 8, 679 input bounds, 692, 710
354, 689
full DAE, 329, 463, 492 input coordinates, 676,
hydraulic actuator, 8, 680,
full pivoting, 490 678–679, 683–684, 692,
704
full rank, 37, 244, 285 698, 721
hydraulic damper, 2
fundamental theorem of input equations, 684, 698, 721
algebra, 38 input space, 690
I
input-output formulation, 689
G identity matrix, 34 input-output model, 679, 683
imbedded error estimation, integrable differential form,
GamEval, 438, 570, 580
303 46
generalized applied force, 368
implicit function theorem, integrating factor, 46, 244
generalized constraint
17, 33, 43, 174, 194, 355, integration step-size, 299
reaction force, 327
683 internal force, 252, 258, 276
generalized coordinate
implicit numerical integration, internal force element, 278,
partitioning, 490
364, 394, 636 280
generalized coordinates, 8,
implicit numerical integration intol, 451
17, 40, 56, 193, 243, 516,
algorithm with friction, 640 inverse function theorem, 45
538, 683, 698, 721
implicit trapezoidal inverse kinematic mapping,
generalized force, 264, 277,
integration, 302, 310, 608, 687
750
inverse kinematics, 722, 724 Lagrange multiplier, 12, 38, momentum, 351
inverse manipulator Jacobian, 264, 331, 361, 394, 602, multibody dynamics, 10
724 618, 624, 658 multibody system, 2
inverse matrix, 37 Lagrange multiplier theorem, multiplication by a scalar, 40
inverse velocity mapping, 724 12, 33, 38, 327 multivariable calculus, 17, 39
isolated singular point, 191 Lagrangian generalized
coordinates, 12, 14–15, 294 N
J Larry Bird Paradox, 555,
n-dimensional Euclidean
557, 583
Jacobian matrix, 47 space, 25
linearly dependent, 36
joint constraint reaction force n-vector, 25
linearly independent, 36, 173,
and torque, 330 near singular configuration,
353
joint contact force, 627 154, 667
Lipschitz continuity, 296
Joint Data Table, 641, 658 negative of a vector, 19
local generalized coordinates,
joint definition frame, neighborhood, 42
173, 354
329–330 Newmark method, 312
local parameterization, 170,
joint definition point, 618 Newton's laws, 241–242,
178
252, 258, 591
locally one to one, 170
K Newton-Euler equations of
locally path connected, 42
motion, 251
kinematic acceleration lock-up configuration, 15,
Newton-Raphson method,
equation, 118–119 147, 190
17, 47, 178, 195, 303,
kinematic analysis, 198 lumped mass model, 422
355–356, 509
kinematic closed loop, 681 lumped mass spring model,
noncentroidal equations of
kinematic configuration 652
motion, 255, 261, 514
space, 684
noncentroidal reference
kinematic constraint, 11, 243 M
frame, 251, 256, 282
kinematic degrees of freedom,
magnitude of a vector, 19 nonholonomic constraint,
518, 520, 540, 542
manipulator, 7 117, 157–158, 244, 497,
kinematic differential
manipulator configuration 504
equation, 501
space, 684, 694, 698 nonintegrability of angular
kinematic driver, 129, 149
manipulator coordinates, 684 velocity, 95
kinematic modeling, 285
massless coupler, 125, 137 nonredundant parallel
kinematic velocity equation,
matrix, 33 manipulator, 682
118–119, 281
matrix algebra, 17, 33 nonsingular forward
kinematically admissible
matrix calculus, 39 kinematics, 685
virtual displacement, 11,
matrix condition number, 47 nonsingular inverse
283
matrix dimension, 33 kinematics, 685
kinematically admissible
matrix product, 35 nonsingular manipulator
virtual velocity, 284, 294
maximal domain of functionality, 676
kinematically parallel
singularity free nonsingular matrix, 37
manipulator, 681
functionality, 181, 689 norm, 41
kinetic degree of freedom,
maze of singularities, 714 normed vector space, 41
518, 520, 540, 542
McPherson strut, 5, 148 not stiff, 575, 583, 598–599,
kinetic energy, 284, 518, 521
mechanical system, 2 612, 647, 667, 670–671
kinetic modeling, 285
mechanism generalized null space, 38, 172, 285, 357
coordinates, 676, 681 null vectors, 498
L
metric space, 41 numerical damping, 559, 583
L-stability, 302 moment of inertia, 249 numerical error control, 17
751
numerical solution error 294 Q
tolerance, 369 particle on a unit sphere, 168,
QAEval, 643, 660
340
qPart, 409, 438, 570, 580
O path connected, 181, 690
quadratically convergent, 195
periods of stiction, 663
ODE, 12, 241, 298 quick return mechanism, 209,
Petzold Imperative, 550,
ODE existence theorem, 296 418, 575, 650
554–555, 557, 574–575,
ODE of motion, 343, 348,
583, 585
359, 457, 504 R
PhiEval, 444
ODEfunct, 409, 438, 570, 581
PJDT, 406 radius of precession, 350
one step methods, 298
planar articulated vehicle, range, 38
one-to-one, 175
516, 538 rank, 37
open ball, 42, 691
planar Cartesian generalized reciprocating motion, 3, 5
open set, 42
coordinates, 56 redundant constraint, 191
optimum step size, 301
planar distance constraint, regular configuration space,
ordinary differential
125 167–168, 172, 346, 353,
equations, 12, 241, 287
planar double pendulum, 498
orientation in space, 58
130, 287, 315, 373, 412, regular manipulator
orientation of bodies, 17
572 configuration space, 686
orientation transformation
Planar Joint Data Table, 199, regular velocity space, 498,
matrix, 17, 54, 125
406 500
orthogonal matrix, 37, 52, 62,
Planar Mass Data Table, 406 relative angle driver, 200
67
planar rigid body, 50 relative distance driver, 200,
orthogonal reference frame,
planar rigid body equations of 218
19
motion, 249 relative rotation driver, 130,
orthogonal transformation
Planar RSDA Data Table, 407 150, 218
matrix, 136
planar slider-crank, 131–132, reparameterization, 345, 375,
orthogonal triad, 62
289, 319 383, 420, 427, 436, 445,
orthogonal vectors, 22
plane of symmetry, 266 452, 529, 535, 599, 608,
orthonormal vectors, 513
PMDT, 406 612, 638
output coordinates, 676, 679,
polar moment of inertia, 254, residual equation, 394, 533
683–684, 698, 721
265, 269 residual form, 561, 563
output equations, 684, 698,
position analysis, 195 resultant force, 242, 261, 263
705, 721
positive definite system mass resultant torque, 261, 263
output space, 690
matrix, 283–284, 505 revolute joint, 126, 143, 199,
positive semidefinite, 268, 619, 626, 628
P
284 revolute-spherical composite
P5 function, 219 precession, 376 joint, 148, 217
parallel axis theorem, 268 principal axes, 269 right-hand orthogonal
parallel manipulator, 9, 678, principal moments of inertia, reference frames, 21
721 269 rigid-body motion, 254
parallel manipulator prismatic joint, 127 RK, 527, 540
singularities, 681 product rule of differentiation, RKF, 527
parallelogram rule, 20 30, 40 RKFN, 308, 404, 415, 561,
Param, 438 products of inertia, 268 566, 572, 575, 579, 583,
parameterization, 168, 171, properly posed, 162–163 585, 647, 649, 658
176, 415 PRSDAT, 407 robot, 2, 7, 290, 676
parameterization of the PTSDAT, 406 rocker arm, 5
regular constraint manifold, roll without slip, 157, 513,
752
516, 519, 522, 538, 540 singular configuration, 15, stage variables, 299, 510,
rotation driver, 133 117, 147, 167, 196, 447, 534, 536
rotational 664 Standard International (SI)
spring-damper-actuator, singular manipulator units, 243
278, 280 configuration, 687 state space approach, 555
row matrix, 33 singular matrix, 37 static equilibrium, 602
row rank, 37 singular point, 191 static friction, 242, 590, 602
row vector, 33 singular value decomposition, steady state, 4
RSDA, 280, 566, 650 173, 354, 533 steering rack, 5
Runge-Kutta, 298, 300 singularities, 15, 681 step size, 301, 510
Runge-Kutta discretization, singularity free domains of step size control, 309
511 functionality, 16, 676, 682, Stewart platform, 678, 732
Runge-Kutta integration of 691 stiction, 242, 590, 598, 602,
second order ODE, 306 singularity maze, 714 613, 667, 669
Runge-Kutta methods, 241, skew symmetric matrix, 26, stiction constraint, 608
298, 304 36, 72 stiction event, 615
Runge-Kutta-Fehlberg, 301, slider-crank, 2–3, 427, 647 stiction force, 602, 606
308 slider-crank manipulator, 684 stiff behavior, 15, 297–298
slip velocity, 609 stiff differential equations,
S SMDT, 434 311
solid modeling, 274 stiff system, 297, 302,
scalar, 19
solution error tolerance, 371 413–414, 572, 599, 615,
scalar product, 22, 26, 41
sparse matrix, 10, 122, 163, 651, 664
SDIRK, 302–303, 310, 400,
282 stiffly-accurate, 303
404, 415, 510, 536, 540,
spatial distance constraint, stiffness, 299, 384, 608, 645
647, 658
137 strong form of Newton's
second order accurate, 312
spatial double pendulum, second law, 252
second order initial-value
385, 443, 585 strut, 5, 148, 217
problem, 296, 304, 315
Spatial Joint Data Table, 215, STSDAT, 434
second order integration
434 sub-Jacobian, 43, 126–128,
algorithm, 304
spatial multibody system, 433 138
second order ODE, 13, 249,
spatial rigid body, 249, 258 submanifold, 690
304, 344, 359
spatial robotic manipulator, subscript notation, 39
second order stage equation,
321 sum of matrices, 34
306
spatial slider-crank, 153, 234, surge waves in a coil spring,
second order tableau, 310
447 422, 652
second order trapezoidal
Spatial TSDA Data Table, symbolic computation, 10
formula, 310
434 symmetric matrix, 36
second-order ordinary
spherical constraint, 137, 216 symmetric top, 345, 376
differential equations, 243
spherical coordinates, 12 system acceleration constraint
sensitivity analysis, 256
spin stabilized top, 376, 400, equations, 122, 162, 194,
serial manipulator, 8,
441, 583 498, 504
676–677
spring constant, 276, 429, 654 system assembly, 195
set valued, 602
spring-damper-actuator, 566 system constraint Jacobian,
shock absorber, 2, 5
square matrix, 34 122, 163, 194, 281
singly diagonal implicit
SRSDAT, 435 system generalized
Runge-Kutta, 302, 310
stability properties, 303 coordinates, 118, 121, 193
singular behavior, 15, 189,
stage equations, 299–300, system variational equation of
637
510, 536 motion, 283
753
system velocity constraint trailing arm, 5 W
equations, 122, 162, 194, transient oscillation, 415
wave speed, 423
282, 498 transient top, 379, 401
weight, 242
system virtual displacement translational joint, 127, 143,
well posed, 296, 360, 392,
conditions, 282 199, 620, 629
455, 461, 463, 505
systems with friction, 391 translational
windshield wiper, 5, 134, 211
spring-damper-actuator,
T 276, 579
X
transpose of a matrix, 33
tangent plane, 169, 174, 341,
trapezoidal algorithm, 302, xPart, 409, 438, 570, 580
353
400, 404, 415, 427, 563,
tangent space, 169, 341, 346
566, 572, 575, 579, 583, Z
tangent space equations of
585, 647, 649, 658, 662,
motion, 351 zero matrix, 34
669
tangent space generalized zero vector, 19
trapezoidal integration, 508,
coordinates, 169, 340–341,
534
346
TSDA, 276, 429, 433, 452,
tangent space Index 0 DAE,
566, 579, 654
409
tangent space kinematics, 391
U
tangent space ODE, 404,
409, 433 unilateral spring, 412
tangent space unit vector, 11, 19
parameterization, 117, universal joint, 144–145, 217
172–173, 354, 458 usolve, 409, 438
Taylor series approximation,
298 V
theorem of the alternative,
variable step size, 299, 319,
189
385
theory of ODE, 296
variational equation of
three-dimensional space, 17
motion, 11, 241, 243, 279,
three-slider mechanism, 645
281–282, 328, 349, 359
three-wheel transporter, 513
vector, 19
three-wheel vehicle, 157
vector product, 23, 27
thrust bearing, 628
vector space, 41, 180
tie rod, 5
vector sum, 20
time derivative of a vector, 29
velocity, 29–30
time grid, 195
velocity analysis, 196
time increment, 244
velocity constraint, 11, 70,
time step, 196
118–119, 157–158, 281,
time step control, 313
682
time-dependent vector, 29
velocity space generalized
topological analysis, 10
coordinates, 500
topological space, 42, 180
velocity tangent space, 498
topological vector space, 42,
virtual displacement, 11, 17,
181
163, 189, 243–245, 253,
total energy, 318, 345, 351,
259, 327, 498, 551
415, 420, 442, 528, 583
virtual work, 241, 243, 276,
trace, 79
629
754

CAKDVolIISecondEd1 10 2021

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

CAKDVolIISecondEd1 10 2021

Uploaded by

Copyright:

Available Formats

COMPUTER-AIDED KINEMATICS AND DYNAMICS

1 ELEMENTS OF MECHANICAL SYSTEM DYNAMICS 1

2 POSITION AND ORIENTATION OF BODIES 17

3 KINEMATICS OF MULTIBODY SYSTEMS 117

4 EQUATIONS OF MOTION FOR BODIES AND MULTIBODY SYSTEMS 241

5 TANGENT SPACE ODE FOR HOLONOMIC SYSTEMS 338

6 TANGENT SPACE ODE FOR NONHOLONOMIC SYSTEMS 496

7 DAE METHODS FOR HOLONOMIC SYSTEMS 550

8 SIMULATION OF FRICTION IN MULTIBODY DYNAMICS 589

9 MANIPULATOR KINEMATICS AND DYNAMICS 676

Figure 1.1.1 Cross Section of a V-2, 4, 6, or 8 Engine

Figure 1.1.3 Windshield Wiper Mechanism

Figure 1.1.4 Automobile

Figure 1.1.5 Automobile Suspension Schematic

Defining the constraint Jacobian as the 3 1 matrix

Figure 1.2.1 Particle on Unit Sphere

mq + qT + mguz = 0 (1.2.6)

Figure 1.2.2 Spherical Coordinates for Particle on Unit Sphere

1 0     2 sin  cos   cos 

Figure 1.2.3 Alternative Coordinates for Particle on Unit Sphere

Figure 2.1.3 Addition of Vectors

Figure 2.1.5 Right-Hand Orthogonal Reference Frame

Figure 2.1.6 Components of a Vector

Figure 2.1.7 Vector Product

2.1.2 Algebraic Vectors

can be written as B = b% , where b = [ - b 23 b13 - b12 ]T ; i.e.,

Direct calculation shows that

Matrix implementation of vector operations permits the systematic organization of

where R and ω are constant. Its velocity is thus

r Tr& = - R 2wcosωtsinωt + R 2wcosωtsinωt = 0

Figure 2.1.9 Particle Moving on Circular Path with Constant Speed

If the length of a vector a ( t ) is constant; i.e. a ( t ) a ( t ) = c , where c is a constant, then

Eq. (2.1.38) yields a& T a + aT a& = 2a& T a = 0 , so

é - Rω2 cosωt ù é Rcosωt ù

or two 1×3 row matrices,

Similarly, for matrices A and B with the same dimension,

or, in terms of the rows of A and columns of B,

Similarly, if A is an m×p matrix, B is a p×q matrix, and C is a q×n matrix,

Multiplication of a matrix A by a scalar α is defined as

Also, if A is an m×p matrix and B is a p×n matrix, then

A set of n ´ 1 column matrices a j , j = 1,L , m , is called linearly dependent if there are

If a set of column matrices is not linearly dependent, it is called linearly independent.

for some b = [β1 L β n ] ¹ 0 . Otherwise they are linearly independent.

åa u = 0 . With u = [ u1 ... u n ] , this is the matrix equation

and if matrices A and B of the same dimension are nonsingular,

A special nonsingular matrix that arises often in kinematics is called an orthogonal

all x that satisfy Eq.(2.2.27) is equivalent to c Î N(B ) ^ = R( B T ) . Thus, there exists an a Î R m

& = d B = é dbij ù = é b& ù

function of q, and f ( q ) = éëf1 ( q ) L f m ( q ) ùû be an m-vector of scalar differentiable functions

careful manipulation, yields the product rule of differentiation

vector, the chain rule of differentiation is

2.2.4 Euclidean Space, Continuity of Functions, and Topologies

With these operations, R n is a vector space.

and the distance between vectors r and s in R n is defined as

where the quantity

is the norm of q Î R n , with the properties

for all q, r, and s in R n . When the dimension n of R n is understood, the subscript n of q n

is called an open ball of radius e centered at q 0 . A set W Ì R n is defined to be an open set in R n

Open sets in R n satisfy the following properties (Mendelson, 1962):

inverse image of W ; i.e., f -1 ( W ) º {q Î R n : f ( q) Î W} , is open (Mendelson, 1962). This result is

such that u0 = h ( v0 ) and

for all v in a neighborhood V 0 of v 0 .

nothing about solutions near (u0 , v0 ) = (0,1) .