Robot and Multibody Dynamics: Analysis and Algorithms

Robot and Multibody Dynamics

Abhinandan Jain

Robot and MultibodyDynamics

Analysis and Algorithms

123

Abhinandan Jain Ph.D.Jet Propulsion Laboratory4800 Oak Grove DrivePasadena, California [email protected]

ISBN 978-1-4419-7266-8 e-ISBN 978-1-4419-7267-5DOI 10.1007/978-1-4419-7267-5Springer New York Dordrecht Heidelberg London

Library of Congress Control Number: 2010938443

c© Springer Science+Business Media, LLC 2011All rights reserved. This work may not be translated or copied in whole or in part without the writtenpermission of the publisher (Springer Science+Business Media, LLC, 233 Spring Street, New York,NY 10013, USA), except for brief excerpts in connection with reviews or scholarly analysis. Use inconnection with any form of information storage and retrieval, electronic adaptation, computer software,or by similar or dissimilar methodology now known or hereafter developed is forbidden.The use in this publication of trade names, trademarks, service marks, and similar terms, even if they arenot identified as such, is not to be taken as an expression of opinion as to whether or not they are subjectto proprietary rights.

Printed on acid-free paper

Springer is part of Springer Science+Business Media (www.springer.com)

In memory of Guillermo Rodriguez,an exceptional scholar and a gentleman.

To my parents,and to my wife, Karen.

Preface

“It is a profoundly erroneous truism, repeated by copybooks and by eminent people whenthey are making speeches, that we should cultivate the habit of thinking of what we aredoing. The precise opposite is the case. Civilization advances by extending the numberof important operations which we can perform without thinking about them. Operations ofthought are like cavalry charges in a battle – they are strictly limited in number, they requirefresh horses, and must only be made at decisive moments.”

Alfred North Whitehead

Robotics, and its need for real-time control, has expanded research in multibodydynamics beyond modeling and systematic formulations, to computational and con-trol issues. While significant progress has been made in these areas, the mathemat-ical complexity of the system dynamics, and the paucity of analytical concepts andmathematical tools to manage the complexity, present imposing challenges.

The body of work known as the spatial operator algebra (SOA) provides suchanalytical and mathematical techniques that address these challenges. The SOA pro-vides spatial operator mathematical constructs, that concisely describe the systemdynamics and enable dynamics analysis in a unified and general setting.

That the equations of multibody dynamics can be described by an algebra of spa-tial operators is certainly of mathematical interest. However, the SOA also providesthe means to manipulate at a high level of abstraction the equations describing multi-body behavior. The significance of this goes beyond the mathematics and is usefulin a practical sense. At any stage of the manipulation of equations, spatially recur-sive algorithms to implement the operator expressions can be readily obtained byinspection. Therefore, the transition from abstract operator mathematics to practicalimplementation is straightforward, and often requires only a simple mental exercise.The focus of this book is on providing a comprehensive and coherent description ofthe SOA methodology and techniques, and of the interplay between mathematicalanalysis, structural properties, and computational algorithms.

The material in this book serves as a point of departure for further exploration oftopics in multibody dynamics. Thus, while the text is written in a style that is acces-sible to a newcomer, it would be best appreciated by practitioners and researchersin the field.

vii

viii Preface

The book is organized into three parts. Part 1, consisting of Chaps. 1–7, is a studyof the dynamics of rigid-link, serial-chain systems. These are the simplest examplesof multi-link systems. They serve to introduce the notion of spatial operators, andto develop the concepts of mapping operator expressions into low-order computa-tional algorithms. In Part 2, consisting of Chaps. 8–13, the operator techniques aregeneralized to include the broader class of multibody systems. Graph theory ideasare used to describe the structure of the operators at a level of abstraction that makesthem independent of the size, the topological structure, and the detailed propertiesof the linkages. Part 3 is an exploration of the application of SOA techniques toadvanced topics. These include methods for partitioning and sub-structuring multi-body models, constraint embedding for transforming closed-chain systems into treesystems, the dynamics of under-actuated systems and free-flying systems, derivationof analytical sensitivity expressions, and diagonalized dynamics formulations.

The text is interspersed with exercises that further explore a topic or presenta related idea. All solutions are provided at the end of the book. Key results aresummarized in lemmas with self-contained assumptions and conclusions.

Rarely is there a single approach or solution that works for the variety of prob-lems encountered when working with robot and multibody system dynamics. Theemphasis of this book therefore is on deepening the theoretical and mathematicalunderstanding of the operator tools, and on the rich tapestry of inter-relationshipsamong them. A single topic is thus sometimes explored from different angles, andthemes and concepts cutting across different contexts are highlighted.

Acknowledgments

My introduction to the fields of robotics and multibody dynamics began whenI joined the Jet Propulsion Laboratory (JPL). Guillermo Rodriguez had recently dis-covered the rich parallels between the mathematical theory underlying optimal fil-tering and estimation theory, and the structure of the dynamical properties of roboticsystems that formed the basis of the SOA. I had the good fortune of joining in thispioneering research. and, until his untimely passing, had the incredible personalexperience of collaborating and working with Guillermo.

Over the years, I have benefited in no small measure from the continuous inter-actions and support from many exceptional colleagues at JPL. Ken Kreutz-Delgadointroduced me to the early developments of the SOA methodology. Guy Man’s vi-sion helped initiate the “real-world” application of the SOA techniques to solvechallenging dynamics modeling problems for NASA’s space missions. I am gratefulto my colleagues at JPL’s DARTS Lab, including Bob Balaram, Jonathan Cameron,Christopher Lim, Marc Pomerantz, Hari Nayar, Jeff Biesiadecki and many otherswho have continued to advance the methodology and make possible its applicationto challenges in space robotics. It has also been a pleasure to work with NagarajanVaidehi in the non-engineering application of the SOA ideas to the molecular dy-namics arena. I would like to thank John Wen and Rich Volpe for their persuasive

Preface ix

encouragement to work on and to publish this book. Last, but not least, I would liketo thank my family, Karen, Aarti, and Nikhil, who valiantly helped with figures,proof-reading, and stylistic suggestions for the text, and for their incredible patienceand forbearance through the months of its preparation.

Contents

Part I Serial-Chain Dynamics

1 Spatial Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.1 Homogeneous Transforms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2 Differentiation of Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.2.1 Vector Derivatives in Rotating Frames . . . . . . . . . . . . . . . . . . . 51.2.2 Rigid Body Vector Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.3 Spatial Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.3.1 Six-Dimensional Spatial Notation . . . . . . . . . . . . . . . . . . . . . . 91.3.2 The Cross-Product for Spatial Vectors . . . . . . . . . . . . . . . . . . . 9

1.4 The Rigid Body Transformation Matrix φ(x,y) . . . . . . . . . . . . . . . . . 111.4.1 Spatial Velocity Transformations . . . . . . . . . . . . . . . . . . . . . . . 121.4.2 Properties of φ(·) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1.5 Spatial Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2 Single Rigid Body Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172.1 Spatial Inertia and Momentum of a Rigid Body . . . . . . . . . . . . . . . . . 17

2.1.1 The Spatial Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172.1.2 The Parallel-Axis Theorem for Spatial Inertias . . . . . . . . . . . . 202.1.3 Spatial Inertia of a Composite Assemblage of Rigid Bodies . 212.1.4 The Spatial Momentum of a Rigid Body . . . . . . . . . . . . . . . . . 21

2.2 Motion Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.2.1 Generalized Coordinates and Velocities . . . . . . . . . . . . . . . . . . 232.2.2 Generalized Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 242.2.3 Generalized Accelerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

2.3 Equations of Motion with Inertial Frame Derivatives . . . . . . . . . . . . . 252.3.1 Equations of Motion with βI = IV(C) . . . . . . . . . . . . . . . . . . 252.3.2 Equations of Motion with βI = IV(z) . . . . . . . . . . . . . . . . . . 27

2.4 Equations of Motion with Body Frame Derivatives . . . . . . . . . . . . . . . 292.5 Equations of Motion with an Inertially Fixed Velocity Reference

Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.6 Comparison of the Different Dynamics Formulations . . . . . . . . . . . . . 33

xi

xii Contents

3 Serial-Chain Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.1 Serial-Chain Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.2 Hinge Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

3.2.1 Hinge Generalized Coordinates . . . . . . . . . . . . . . . . . . . . . . . . 373.2.2 Relative and Absolute Coordinates . . . . . . . . . . . . . . . . . . . . . . 383.2.3 Hinge Generalized Velocities . . . . . . . . . . . . . . . . . . . . . . . . . . 383.2.4 Examples of Hinges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

3.3 Serial-Chain Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 423.3.1 Serial-Chain Configuration Kinematics . . . . . . . . . . . . . . . . . . 423.3.2 Serial-Chain Differential Kinematics . . . . . . . . . . . . . . . . . . . . 433.3.3 Differential Kinematics with Bk �= Ok . . . . . . . . . . . . . . . . . . 45

3.4 Spatial Operators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 473.4.1 The φ Spatial Operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 493.4.2 Velocity Operator Expression . . . . . . . . . . . . . . . . . . . . . . . . . . 503.4.3 The φ Spatial Operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

3.5 Recursions Associated with the φ Operator . . . . . . . . . . . . . . . . . . . . 513.6 The Jacobian Operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

4 The Mass Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 574.1 Mass Matrix of a Serial-Chain System . . . . . . . . . . . . . . . . . . . . . . . . . 57

4.1.1 Kinetic Energy of the Serial-Chain . . . . . . . . . . . . . . . . . . . . . . 574.1.2 Composite Rigid Body Inertias . . . . . . . . . . . . . . . . . . . . . . . . . 594.1.3 Decomposition of φMφ∗ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 614.1.4 O(N2) Algorithm for Computing the Mass Matrix . . . . . . . . 634.1.5 Relationship to the Composite Rigid Body Method . . . . . . . . 65

4.2 Lagrangian Approach to the Equations of Motion . . . . . . . . . . . . . . . . 664.2.1 Properties of M and C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 674.2.2 Hamiltonian Form of the Equations of Motion . . . . . . . . . . . . 704.2.3 Transformation of Lagrangian Coordinates . . . . . . . . . . . . . . . 70

5 Serial-Chain Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 755.1 Equations of Motion for a Typical Link . . . . . . . . . . . . . . . . . . . . . . . . 75

5.1.1 Expression for the Spatial Acceleration α(k) . . . . . . . . . . . . . 775.1.2 Overall Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . 815.1.3 Spatial Operators with Body Frame Derivatives but Bk �= Ok 83

5.2 Inclusion of External Forces and Gravity . . . . . . . . . . . . . . . . . . . . . . . 855.2.1 Inclusion of External Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . 855.2.2 Compensating for External Forces . . . . . . . . . . . . . . . . . . . . . . 865.2.3 Inclusion of Gravitational Forces . . . . . . . . . . . . . . . . . . . . . . . 87

5.3 Inverse Dynamics of Serial-Chains . . . . . . . . . . . . . . . . . . . . . . . . . . . . 885.3.1 Newton–Euler Inverse Dynamics Algorithm . . . . . . . . . . . . . . 885.3.2 Computing the Mass Matrix Using Inverse Dynamics . . . . . . 905.3.3 Composite Rigid Body Inertias Based Inverse Dynamics . . . 91

Contents xiii

5.4 Equations of Motion with an Inertially Fixed Velocity ReferenceFrame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 935.4.1 Relationship Between φI and φ . . . . . . . . . . . . . . . . . . . . . . . . 96

6 Articulated Body Model for Serial Chains . . . . . . . . . . . . . . . . . . . . . . . . 976.1 Alternate Models for Multibody Systems . . . . . . . . . . . . . . . . . . . . . . . 97

6.1.1 Terminal Body Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 976.1.2 Composite Body Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 986.1.3 Articulated Body Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

6.2 The P(k) Articulated Body Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1006.2.1 Induction Argument for P(k) . . . . . . . . . . . . . . . . . . . . . . . . . . 1006.2.2 The D(k) and G(k) Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . 1016.2.3 The τ(k) and τ(k) Projection Matrices . . . . . . . . . . . . . . . . . . 1026.2.4 The P+(k) Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1046.2.5 Conclusion of the Induction Argument for P(k) . . . . . . . . . . . 105

6.3 Articulated Body Model Force Decomposition . . . . . . . . . . . . . . . . . . 1076.3.1 The ε(k) Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1096.3.2 Acceleration Relationships . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

6.4 Parallels with Estimation Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1116.4.1 Process Covariances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1126.4.2 Optimal Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1126.4.3 Optimal Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1136.4.4 Extensions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114

7 Mass Matrix Inversion and AB Forward Dynamics . . . . . . . . . . . . . . . . 1157.1 Articulated Body Spatial Operators . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

7.1.1 Some Operator Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1177.1.2 Innovations Operator Factorization of the Mass Matrix . . . . . 1207.1.3 Operator Inversion of the Mass Matrix . . . . . . . . . . . . . . . . . . 121

7.2 Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1227.2.1 O(N) AB Forward Dynamics Algorithm . . . . . . . . . . . . . . . . 123

7.3 Extensions to the Forward Dynamics Algorithm . . . . . . . . . . . . . . . . . 1287.3.1 Computing Inter-Link f Spatial Forces . . . . . . . . . . . . . . . . . . . 1287.3.2 Including Gravitational Accelerations . . . . . . . . . . . . . . . . . . . 1287.3.3 Including External Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1297.3.4 Including Implicit Constraint Forces . . . . . . . . . . . . . . . . . . . . 130

Part II General Multibody Systems

8 Graph Theory Connections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1358.1 Directed Graphs and Trees . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1358.2 Adjacency Matrices for Digraphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

8.2.1 Properties of Digraph Adjacency Matrices . . . . . . . . . . . . . . . 1388.2.2 Properties of Tree Adjacency Matrices . . . . . . . . . . . . . . . . . . 1408.2.3 Properties of Serial-Chain Adjacency Matrices . . . . . . . . . . . 140

8.3 Block Weighted Adjacency Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . 141

xiv Contents

8.4 BWA Matrices for Tree Digraphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1428.4.1 The 1-Resolvent of Tree BWA Matrices . . . . . . . . . . . . . . . . . 143

8.5 Similarity Transformations of a Tree BWA Matrix . . . . . . . . . . . . . . . 1478.5.1 Permutation Similarity Transformations . . . . . . . . . . . . . . . . . 1488.5.2 Similarity-Shift Transformations . . . . . . . . . . . . . . . . . . . . . . . 149

8.6 Multibody System Digraphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1508.6.1 BWA Matrices and Serial-Chain, Rigid Body Systems . . . . . 151

8.6.1.1 Eφ Is a BWA Matrix . . . . . . . . . . . . . . . . . . . . . . . . . 1538.6.2 Non-Canonical Serial-Chains . . . . . . . . . . . . . . . . . . . . . . . . . . 154

8.7 BWA Matrices and Tree-Topology, Rigid Body Systems . . . . . . . . . . 1548.7.1 Equations of Motion for Tree Topology Systems . . . . . . . . . . 155

9 SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1599.1 SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160

9.1.1 Definition of SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1609.1.2 Existence of SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1619.1.3 Generalizations of SKO Models . . . . . . . . . . . . . . . . . . . . . . . . 161

9.2 SPO Operator/Vector Products for Trees . . . . . . . . . . . . . . . . . . . . . . . 1629.2.1 SKO Model O(N) Newton–Euler Inverse Dynamics . . . . . . . 165

9.3 Lyapunov Equations for SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . 1669.3.1 Forward Lyapunov Recursions for SKO Models . . . . . . . . . . 1669.3.2 Mass Matrix Computation for an SKO Model . . . . . . . . . . . . 1689.3.3 Backward Lyapunov Recursions for SKO Models . . . . . . . . . 170

9.4 Riccati Equations for SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1749.4.1 The Eψ and ψ SKO and SPO Operators . . . . . . . . . . . . . . . . . 1759.4.2 Operator Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176

9.5 SKO Model Mass Matrix Factorization and Inversion . . . . . . . . . . . . 1799.5.1 O(N) AB Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 180

9.6 Generalized SKO Formulation Process . . . . . . . . . . . . . . . . . . . . . . . . . 1829.6.1 Procedure for Developing an SKO Model . . . . . . . . . . . . . . . . 1839.6.2 Potential Non-Tree Topology Generalizations . . . . . . . . . . . . 185

10 Operational Space Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18710.1 Operational Space Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . 187

10.1.1 Physical Interpretation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18910.1.2 Operational Space Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189

10.2 Structure of the Operational Space Inertia . . . . . . . . . . . . . . . . . . . . . . 19010.2.1 TheΩ Extended Operational Space Compliance Matrix . . . . 19010.2.2 Decomposition ofΩ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19110.2.3 ComputingΛ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19310.2.4 The Υ Operational Space Compliance Kernel . . . . . . . . . . . . . 19410.2.5 Simplifications for Serial-Chain Systems . . . . . . . . . . . . . . . . 19710.2.6 Explicit Computation of the Mass Matrix Inverse M−1 . . . . . 198

10.3 The Operational Space Cos Coriolis/Centrifugal Term . . . . . . . . . . . . 19910.3.1 The U and U⊥ Projection Operators . . . . . . . . . . . . . . . . . . . . . 199

Contents xv

10.3.2 Computing Cos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20210.4 Divide and Conquer Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 204

11 Closed-Chain Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20911.1 Modeling Closed-Chain Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 209

11.1.1 Types of Bilateral Motion Constraints . . . . . . . . . . . . . . . . . . . 20911.1.2 Constrained System Forward Dynamics Strategies . . . . . . . . 211

11.2 Augmented Approach for Closed-Chain Forward Dynamics . . . . . . . 21211.2.1 Move/Squeeze Decompositions . . . . . . . . . . . . . . . . . . . . . . . . 21411.2.2 Augmented Dynamics with Loop Constraints . . . . . . . . . . . . . 21511.2.3 Dual-Arm System Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219

11.3 Projected Closed-Chain Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22111.4 Equivalence of Augmented and Projected Dynamics . . . . . . . . . . . . . 22211.5 Unilateral Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224

11.5.1 Complementarity Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22511.5.2 Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 227

12 Systems with Geared Links . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22912.1 Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 230

12.1.1 Reformulated Equations of Motion . . . . . . . . . . . . . . . . . . . . . 23212.1.2 Eliminating the Geared Constraint . . . . . . . . . . . . . . . . . . . . . . 233

12.2 SKO Model for Geared Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23412.2.1 Expression for the Mass Matrix . . . . . . . . . . . . . . . . . . . . . . . . 236

12.3 Computation of the Mass Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23712.3.1 Optimized Composite Body Inertia Algorithm . . . . . . . . . . . . 238

12.4 O(N) AB Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23912.4.1 Mass Matrix Factorization and Inversion . . . . . . . . . . . . . . . . . 23912.4.2 Recursive AB Forward Dynamics Algorithm . . . . . . . . . . . . . 24112.4.3 Optimization of the Forward Dynamics Algorithm . . . . . . . . 242

13 Systems with Link Flexibility . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24513.1 Lumped Mass Model for a Single Flexible Body . . . . . . . . . . . . . . . . 245

13.1.1 Equations of Motion of the Ojk Node . . . . . . . . . . . . . . . . . . . . 246

13.1.2 Nodal Equations of Motion for the kth Flexible Body . . . . . 24713.1.3 Recursive Relationships Across the Flexible Bodies . . . . . . . 248

13.2 Modal Formulation for Flexible Bodies . . . . . . . . . . . . . . . . . . . . . . . . 24913.2.1 Modal Mass Matrix for a Single Body . . . . . . . . . . . . . . . . . . . 25113.2.2 Recursive Relationships Using Modal Coordinates . . . . . . . . 25213.2.3 Recursive Propagation of Accelerations . . . . . . . . . . . . . . . . . 25313.2.4 Recursive Propagation of Forces . . . . . . . . . . . . . . . . . . . . . . . 25413.2.5 Overall Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . 255

13.3 SKO Models for Flexible Body Systems . . . . . . . . . . . . . . . . . . . . . . . 25513.3.1 Operator Expression for the System Mass Matrix . . . . . . . . . 25713.3.2 Illustration of the SKO Formulation Procedure . . . . . . . . . . . . 257

13.4 Inverse Dynamics Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 259

xvi Contents

13.5 Mass Matrix Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26113.6 Factorization and Inversion of the Mass Matrix . . . . . . . . . . . . . . . . . . 26213.7 AB Forward Dynamics Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264

13.7.1 Simplified Algorithm for the Articulated Body Quantities . . 26413.7.2 Simplified AB Forward Dynamics Algorithm . . . . . . . . . . . . . 266

Part III Advanced Topics

14 Transforming SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27314.1 Partitioning Digraphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273

14.1.1 Partitioning by Path-Induced Sub-Graphs . . . . . . . . . . . . . . . . 27414.2 Partitioning SKO Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275

14.2.1 Partitioning SKO Model Operators . . . . . . . . . . . . . . . . . . . . . 27514.2.2 Partitioning of an SKO Model . . . . . . . . . . . . . . . . . . . . . . . . . 277

14.3 SPO Operator Sparsity Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27814.3.1 Decomposition into Serial-Chain Segments . . . . . . . . . . . . . . 27814.3.2 Sparsity Structure of the EA SKO Matrix . . . . . . . . . . . . . . . . 28014.3.3 Sparsity Structure of the A Matrix . . . . . . . . . . . . . . . . . . . . . . 28114.3.4 Sparsity Structure of the M Mass Matrix . . . . . . . . . . . . . . . . 282

14.4 Aggregating Sub-Graphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28214.4.1 Edge and Node Contractions . . . . . . . . . . . . . . . . . . . . . . . . . . . 28314.4.2 Tree Preservation After Sub-Graph Aggregation . . . . . . . . . . 28414.4.3 The SA Aggregation Sub-Graph . . . . . . . . . . . . . . . . . . . . . . . 287

14.5 Transforming SKO Models Via Aggregation . . . . . . . . . . . . . . . . . . . . 28714.5.1 SKO Operators After Body Aggregation . . . . . . . . . . . . . . . . . 28714.5.2 SKO Model for the TS Aggregated Tree . . . . . . . . . . . . . . . . 290

14.6 Aggregation Relationships at the Component Level . . . . . . . . . . . . . . 29214.6.1 Velocity Relationships . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29314.6.2 Acceleration Relationships . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29414.6.3 Force Relationships . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295

15 Constraint Embedding . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29715.1 Constraint Embedding Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 297

15.1.1 Embedding Constraint Sub-Graphs . . . . . . . . . . . . . . . . . . . . . 29915.2 Examples of Constraint Embedding . . . . . . . . . . . . . . . . . . . . . . . . . . . 303

15.2.1 Geared Links . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30415.2.2 Planar Four-Bar Linkage System (Terminal Cut) . . . . . . . . . . 30515.2.3 Planar Four-Bar Linkage System (Internal Cut) . . . . . . . . . . . 306

15.3 Recursive AB Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30715.3.1 Articulated Body Inertias for the Aggregated System . . . . . . 30715.3.2 Mass Matrix Factorization and Inversion . . . . . . . . . . . . . . . . . 30815.3.3 AB Forward Dynamics Algorithm . . . . . . . . . . . . . . . . . . . . . . 308

15.4 Computing XS and XS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30915.5 Generalization to Multiple Branches and Cut-Edges . . . . . . . . . . . . . . 311

Contents xvii

16 Under-Actuated Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31316.1 Modeling of Under-Actuated Manipulators . . . . . . . . . . . . . . . . . . . . . 314

16.1.1 Decomposition into Passive and Active Systems . . . . . . . . . . 31516.1.2 Partitioned Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . 31616.1.3 Spatial Operator Expression for M−1

pp . . . . . . . . . . . . . . . . . . . 31716.1.4 Operator Expressions for S Blocks . . . . . . . . . . . . . . . . . . . . . . 319

16.2 O(N) Generalized Dynamics Algorithms . . . . . . . . . . . . . . . . . . . . . . 32016.2.1 Application to Prescribed Motion Dynamics . . . . . . . . . . . . . . 324

16.3 Jacobians for Under-Actuated Systems . . . . . . . . . . . . . . . . . . . . . . . . . 32416.3.1 The Generalized Jacobian JG . . . . . . . . . . . . . . . . . . . . . . . . . . 32516.3.2 Computed-Torque for Under-Actuated Systems . . . . . . . . . . . 32616.3.3 The Disturbance Jacobian JD . . . . . . . . . . . . . . . . . . . . . . . . . . 327

16.4 Free-Flying Systems as Under-Actuated Systems . . . . . . . . . . . . . . . . 32916.4.1 Integrals of Motion for Free-Flying Systems . . . . . . . . . . . . . 329

17 Free-Flying Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33117.1 Dynamics of Free-Flying Manipulators . . . . . . . . . . . . . . . . . . . . . . . . 331

17.1.1 Dynamics with Link n as Base-Body . . . . . . . . . . . . . . . . . . . 33117.1.2 Dynamics with Link 1 as Base-Body . . . . . . . . . . . . . . . . . . . . 33217.1.3 Direct Computation of Link Spatial Acceleration . . . . . . . . . . 33617.1.4 Dynamics with Link k as Base-Body . . . . . . . . . . . . . . . . . . . . 336

17.2 The Base-Invariant Forward Dynamics Algorithm . . . . . . . . . . . . . . . 33717.2.1 Parallels with Smoothing Theory . . . . . . . . . . . . . . . . . . . . . . . 33817.2.2 Simplifications Using Non-Minimal Coordinates . . . . . . . . . . 33917.2.3 Computational Issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34017.2.4 Extensions to Tree-Topology Manipulators . . . . . . . . . . . . . . . 340

17.3 SKO Model with kth Link as Base-Body . . . . . . . . . . . . . . . . . . . . . . 34117.3.1 Generalized Velocities with kth Link as the Base-Body . . . . 34117.3.2 Link Velocity Recursions with kth Link as the Base-Body . 34217.3.3 Partitioned System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34317.3.4 Properties of a Serial-Chain SKO Operator . . . . . . . . . . . . . . . 34417.3.5 Reversing the SKO Operator . . . . . . . . . . . . . . . . . . . . . . . . . . . 34517.3.6 Transformed SKO Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 347

17.4 Base-Invariant Operational Space Inertias . . . . . . . . . . . . . . . . . . . . . . 348

18 Spatial Operator Sensitivities for Rigid-Body Systems . . . . . . . . . . . . . . 35318.1 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353

18.1.1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35318.1.2 Identities for Vv . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354

18.2 Operator Time Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35618.2.1 Time Derivatives of φ(k+ 1,k), H(k) andM(k) . . . . . . . . . 35618.2.2 Time Derivatives with Ok �= Bk . . . . . . . . . . . . . . . . . . . . . . . . 35818.2.3 Time Derivative of the Mass Matrix . . . . . . . . . . . . . . . . . . . . . 360

18.3 Operator Sensitivities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36118.3.1 The ˜H

ω

�i , ˜Hω

≺i , and ˜Hω

=i Operators . . . . . . . . . . . . . . . . . 362

xviii Contents

18.4 Mass Matrix Related Quantities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36518.4.1 Sensitivity of φMφ∗ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36518.4.2 Sensitivity of the Mass Matrix Mθi . . . . . . . . . . . . . . . . . . . . . 36518.4.3 Sensitivity of the Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . 36618.4.4 Equivalence of Lagrangian and Newton–Euler Dynamics . . . 367

18.5 Time Derivatives of Articulated Body Quantities . . . . . . . . . . . . . . . . 36818.6 Sensitivity of Articulated Body Quantities . . . . . . . . . . . . . . . . . . . . . . 37318.7 Sensitivity of Innovations Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375

19 Diagonalized Lagrangian Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37919.1 Globally Diagonalized Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37919.2 Diagonalization in Velocity Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 382

19.2.1 Coriolis Force Does No Work . . . . . . . . . . . . . . . . . . . . . . . . . . 38419.2.2 Rate of Change of the Kinetic Energy . . . . . . . . . . . . . . . . . . . 384

19.3 The Innovations Factors as Diagonalizing Transformations . . . . . . . . 38519.3.1 Transformations Between θ and η . . . . . . . . . . . . . . . . . . . . . . 386

19.4 Expression for C(θ,η) for Rigid-Link Systems . . . . . . . . . . . . . . . . . . 38719.4.1 Closed-Form Expression for mη . . . . . . . . . . . . . . . . . . . . . . . 38719.4.2 Operator Expression for C(θ,η) . . . . . . . . . . . . . . . . . . . . . . . . 38919.4.3 Decoupled Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 392

19.5 Un-normalized Diagonalized Equations of Motion . . . . . . . . . . . . . . . 39319.5.1 O(N) Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394

Useful Mathematical Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 397A.1 3-Vector Cross-Product Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 397A.2 Matrix and Vector Norms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 397A.3 Schur Complement and Matrix Inverse Identities . . . . . . . . . . . . . . . . 398A.4 Matrix Inversion Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 400A.5 Matrix Trace Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 400A.6 Derivative and Gradient Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401

A.6.1 Function Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401A.6.2 Vector Gradients . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401A.6.3 Matrix Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 402

Attitude Representations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403B.1 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403B.2 Angle/Axis Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 404B.3 Unit Quaternions/Euler Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . 406

B.3.1 The E+(q) and E−(q) Matrices . . . . . . . . . . . . . . . . . . . . . . . . 408B.3.2 Quaternion Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . 409B.3.3 Quaternion Differential Kinematics . . . . . . . . . . . . . . . . . . . . . 410

B.4 Gibbs Vector Attitude Representations . . . . . . . . . . . . . . . . . . . . . . . . . 412

Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475

Contents xix

List of Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 483

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 493

Part ISerial-Chain Dynamics

Chapter 1Spatial Vectors

This chapter establishes notation and several foundational concepts required forthe study of multibody kinematics and dynamics. Beginning with the notion ofcoordinate-free representations, we introduce homogeneous transformations, spatialvectors and their properties. Spatial vectors are used to define spatial velocities, ac-celerations and forces for coordinate frames and bodies. Rigid body transformationmatrices that transform spatial velocities and forces across frames on a rigid bodyare also introduced. This chapter also studies time derivatives of spatial quantitieswith respect to different frames.

1.1 Homogeneous Transforms

The location of coordinate frames and points are described using 3-vectors.Figure 1.1 shows a pair of frames, F and G, and a point O. With p(F,O) de-

FG

p(F,O) p(G,O)

l(F,G)

Fig. 1.1 Relationship between two coordinate frames

noting the 3-vector position of O with respect to the F frame, its location withrespect to the G frame is

A. Jain, Robot and Multibody Dynamics: Analysis and Algorithms, 3DOI 10.1007/978-1-4419-7267-5 1, c© Springer Science+Business Media, LLC 2011

4 1 Spatial Vectors

p(F,O) = l(F,G)+p(G,O) (1.1)

l(F,G) denotes the vector from F to G. Equation (1.1) is a coordinate-free de-scription of relationship between the vectors. While coordinate-free notation is sim-pler, actual computations require working with coordinate-frame representationsof vectors. For a vector x, the notation Fx denotes its representation in the F frame.Thus, using it, (1.1) can be stated using coordinate-frame representations as follows:

Fp(F,O) =Fl(F,G)+

FRG

Gp(G,O) (1.2)

FRG denotes the rotation matrix1 that transforms the representation of a 3-vectorin frame G into its representation in frame F. Binary operations such as addi-tion require that the respective vector representations be in a common coordinate-frame. Since coordinate-free representations are more general and notationallysimpler, we use them by default, and resort to specific coordinate representationsonly when numeric evaluations are required. The distinction between coordinate-free and coordinate-dependent forms of vectors are also discussed in [69, 185].

Define the homogeneous transform matrix, FTG ∈R4×4, as

FTG

�=

(

FRGFl(F,G)

0 1

)

(1.3)

Using it, (1.2) can be restated in the following matrix form:[

Fp(F,O)

1

]

=FTG

[

Gp(G,O)

1

]

(1.4)

Thus FTG defines the transformation relationship between the coordinate-frame rep-

resentations of position vectors with respect to the F and G frames. It is easy toverify that

FT

−1G

=G

TF and FTG

GTH =

FTH (1.5)

These imply that homogeneous transforms form a group,2 with the group composi-tion operation being matrix multiplication. The identity element is simply the 4×4identity matrix.

1 A rotation matrix (also known as a direction cosine matrix) is a 3× 3 orthogonal matrix withdeterminant 1.2 The group of homogeneous transforms in fact form a Lie group. This, and other differentialgeometric connections, are explored more thoroughly in [128, 162].

1.2 Differentiation of Vectors 5

1.2 Differentiation of Vectors

Unlike scalar functions, the derivative of a vector with respect to a scalar variable(often time) is defined with respect to a coordinate frame. Thus, given a frame, F,and a vector x(s) that is a function of a scalar variable “s”, its derivative with respectto “s” as observed in frame F is defined as the derivative of the Fx(s) coordinaterepresentation with respect to s, that is

dFx

ds�=

d[

Fx(s)]

ds=

⎡

⎢

⎣

dx1(s)ds

dx2(s)ds

dx3(s)ds

⎤

⎥

⎦(1.6)

This vector derivative is itself a vector, and, strictly speaking, (1.6) defines thecoordinate-representation of this derivative vector in frame F. The coordinate-representation of the derivative vector in a different frame G can be obtained asfollows:

GdFx

ds=

GRF

FdFx

ds(1.7)

Since the representation of vectors in different frames is different, the vector deriva-tives with respect to different frames can also be different. In general, when thelocation and orientation of the frames themselves also depend on “s”,

dFx

ds�= dGx

ds

The clear identification of the frame in which a vector derivative is being carriedout is essential. In the multibody dynamics context, we will encounter numerousinstances where time derivatives of vectorial quantities with respect to different ro-tating and translating frames are used during kinematics and dynamics analysis.

1.2.1 Vector Derivatives in Rotating Frames

Let us now examine the important special case where the vector derivatives arewith respect to time, “t”, and where frames are rotating with respect to each other.The 3-vector angular velocity of a frame with respect to to another frame definesthe rate of change of the relative attitude of the pair of frames. Specifically, withω(F,G) denoting the angular velocity of a frame G with respect to another frameF, the following differential equation governs the time evolution of the FRG rotationmatrix [69, 96]:

dFRG

dt=

Fω(F,G)FRG =

FRG

Gω(F,G) (1.8)

6 1 Spatial Vectors

The notation, ˜l, (alternatively l∼) for a 3-vector l denotes the following 3 × 3dimensional skew-symmetric matrix:

˜l= l∼�=

⎛

⎜

⎝

0 −c b

c 0 −a

−b a 0

⎞

⎟

⎠where l

�=

⎡

⎢

⎣

a

b

c

⎤

⎥

⎦, (1.9)

The ˜lmatrix is closely related to the 3-vector cross-product operation l⊗(·), so thatfor an arbitrary 3-vector x:

l⊗x= ˜l x (1.10)

Unlike linear velocities which are defined for points, angular velocities are al-ways defined for frames. The ω(F,G) angular velocity plays an important role inrelating vector time derivatives in different frames. Differentiating both sides ofFx= FRG

Gx with respect to time t leads to

d[

Fx]

dt1.8=

FRG

d[

Gx]

dt+

FRG

Gω(F,G)Gx (1.11)

In coordinate-free notation, (1.11) can be expressed as

dFx

dt=

dGx

dt+ ω(F,G)x (1.12)

Equation (1.12) defines the relationship between the time derivatives of a 3-vectorin different rotating frames. Observe that the time derivative vectors are equal whenthe pair of frames are not rotating with respect to each other.

1.2.2 Rigid Body Vector Derivatives

The motion of a rigid body involves both rotation and translation with respect to theinertial frame. Due to the rigidity assumption, the relative position and attitude offrames fixed to a rigid body remain constant at all times. As illustrated in Fig. 1.2,let x and y denote a pair of points attached to a rigid body with body fixed frame B.Assume that the body is rotating with angular velocityω with respect to the inertialframe I. With l(x,y) denoting the 3-vector from point x to point y, we have

l(I,y) = l(I,x)+ l(x,y) (1.13)

Then differentiating (1.13) with respect to time we obtain

dIl(I,y)dt

=dIl(I,x)

dt+

dIl(x,y)dt

(1.14)

1.2 Differentiation of Vectors 7

x

y

l(x,y)

B bodyframe

I inertialframe

Fig. 1.2 Two points on a single rigid body

The linear velocity of a point x with respect to frame I is denoted, v(x), anddefined as

v(x)�=

dIl(I,x)dt

Using this in (1.14) leads to

v(y)1.14,1.11

= v(x)+dBl(x,y)

dt+ ω l(x,y) = v(x)+ ω l(x,y) (1.15)

The last equality used the constancy of l(x,y) in the body frame B. Thus, physically,while all frames on a rigid body move with the same angular velocity, i.e.,

ω(y) =ω(x) (1.16)

the points on a rigid body move with different linear velocities when observed froma frame rotating with respect to the body.

Equation (1.16) has used a notational flourish which is worth highlighting. Sincex is a point, it does not really have an angular velocity. ω(x) and ω(y) representthe angular velocities of the rigid body frame B rigidly translated to the points xand y, respectively. In other words, in the context of a rigid body, there is a frameimplicitly located at every point on the body defined by translating the body frameto the point. Thus, ω(x) for a point on a rigid body refers to the angular velocityof this frame at x. We will by default continue to use this notational shorthand forpoints on a rigid body – for angular velocities as well as other types of vectorialquantities – except in cases where a frame is explicitly identified. The followingexercise examines acceleration level relationships for vectors and rotating frames.

Exercise 1.1 Position and velocity vector derivatives.Let I and B denote a pair of frames. Let v(B) and ω(B) denote the linear andangular velocities of frame B with respect to frame I. Also, the linear and angularinertial frame accelerations, defined as the I frame derivatives of the linear and

8 1 Spatial Vectors

angular velocities, respectively are denoted as:

a(·) �=

dIv(·)dt

, and α(·) �=

dIω(·)dt

1. Show that

dBω(B)

dt= α(B), and

dBv(B)

dt= a(B)− ω(B)v(B) (1.17)

2. Let B denote the body-fixed frame of a rigid body that is rotating with angularvelocity,ω, with respect to the inertial frame, I. With x and y denoting a pair ofpoints on the rigid body, as shown in Fig. 1.2, show that the linear accelerationof the points are related by:

a(y) = a(x)+ α(x)l(x,y)+ ωω l(x,y) (1.18)

3. Now let P denote a particle moving on the body with velocity δv(P) with respectto the B body frame. Let y denote the instantaneous location of P on the body.Show that the linear velocity, v(P), and linear acceleration, a(P), of the particleP with respect to the inertial frame I are given by

v(P) = v(y)+δv(P) (1.19a)

a(P) = a(y)+dBδv(P)

dt+ 2ω δv(P) (1.19b)

where v(y) and a(y) are defined by (1.15) and (1.18).

1.3 Spatial Vectors

In this section, we introduce two types of quantities:

• Spatial vectors, which combine 3-vector translational and rotational quantities(e.g., velocities, forces) into a single 6-dimensional vector.

• The rigid body transformation matrix φ(·), which relates the spatial velocity andforce at a frame on a rigid body to the spatial velocity and force at another frameon the same body.

The spatial notation used here is based upon references [51,140,151]. A slight vari-ant, based on the classical concepts of screws and wrenches, was used originally in[47, 48].

1.3 Spatial Vectors 9

1.3.1 Six-Dimensional Spatial Notation

Use of 6-dimensional spatial notation is a step towards simpler expressions ofrigid body kinematics and dynamics equations. A spatial vector is defined as a6-vector consisting of a pair of component angular and linear 3-vectors. A concreteexample of a spatial vector is the 6-dimensional spatial velocity, V(x), of a frame xdefined as

V(x)�=

[

ω(x)

v(x)

]

(1.20)

V(x) is a 6-dimensional vector consisting of the pair of ω(x) angular velocityand the v(x) linear velocity 3-vectors. Strictly speaking spatial vectors live in theR3×R3 space, but for notational simplicity we will treat them as members of theR6 space. Like angular velocities, spatial velocities are always defined for frames,and not for points. However, in the context of a rigid body, the spatial velocity ofa point x on the body will denote by default the spatial velocity of the body frametranslated to point x on the rigid body.

There will be times when we need to work with just the angular or just the linearcomponents of a spatial vector. So for a spatial vector X, Xω will denote the spatialvector with just the angular component of X, while Xv will denote the spatial vectorwith just the linear component ofX. In particular for the spatial velocity vector V(x),

Vω(x)

�=

[

ω(x)

0

]

and Vv(x)

�=

[

0

v(x)

]

(1.21)

Clearly the following identity is always true:

X= Xω+Xv (1.22)

1.3.2 The Cross-Product for Spatial Vectors

With z�=

[

x

y

]

and c�=

[

a

b

]

denoting a pair of spatial vectors, the 6-dimensional

spatial vector cross-product is defined as:

z⊗ c �= zc=

(

xa

ya+ xb

)

where z�=

(

x 03

y x

)

∈R6×6 (1.23)

The ˜( ) operator defined above for spatial vectors is a natural generalization of the3-dimensional cross-product operator defined in (1.9) for 3-vectors. The followingexercise establishes identities for the spatial vector cross-product that are analogousto the ones for 3-vectors in (A.1) in Appendix A.

10 1 Spatial Vectors

Exercise 1.2 Spatial vector cross-product identities.Verify the following spatial vector identities:

ÃA = 0

ÃB= −˜BA (skew-symmetry)

Ã ˜BC+ ˜B ˜CA+ ˜C ÃB= 0 (Jacobi identity)

Ã˜B− ˜B Ã =˜

ÃB (commutator)

(1.24)

where A, B and C are arbitrary spatial vectors.We now define the related ( ) and the skew-symmetric ( ) operators for a spatial

vector z as follows:

z�=

(

x y

03 x

)

and z�=

(

x y

y 03

)

(1.25)

It is easy to verify the following relationships:

z = − z∗, z∗z = 0 and zz =

[

0xy

]

(1.26)

The following exercise establishes relationships between these different spatial vec-tor operators.

Exercise 1.3 Relationship between the ˜( ), ( ) and ( ) operators.Verify the following identities relating the ˜( ), ( ) and ( ) spatial vectors opera-tors:

A∗B = −B∗A (1.27a)Ã∗B = BA= −B∗A = −AB (1.27b)

A∗˜BC = B∗CA= C∗

AB (1.27c)

The following exercise extends the 3-vector time derivative relationship for ro-tating frames in (1.12) to a corresponding relationship for spatial vectors.

Exercise 1.4 Time derivative relationship for spatial vectors.Show that the generalization of (1.12) for the time derivative of a spatial vectorX(t)

is given bydFX

dt=

dGX

dt+ ˜Vω

(F,G) X (1.28)

where V(F,G) denotes the spatial velocity of frame G with respect to frame F.

1.4 The Rigid Body Transformation Matrix φ(x,y) 11

1.4 The Rigid Body Transformation Matrix φ(x,y)

The 6× 6 dimensional rigid body transformation matrix, φ(x,y), for a pair offrames x and y is defined in coordinate-free form as

φ(x,y)�=

(

I3 ˜l(x,y)

03 I3

)

(1.29)

where l(x,y) denotes the vector between the x and y frames. The symbol I3 de-notes the 3× 3 identity matrix. The inertial frame coordinate-frame representationof φ(x,y) has the following form:

φ(x,y) =

(

I3I˜l (x,y)

03 I3

)

(1.30)

In general there is no rotational component in a coordinate-frame representation ofφ(x,y) when it transforms spatial vectors expressed in the same frame.

Clearly, other coordinate-frame representations of φ(x,y) are possible. The fol-lowing coordinate-frame representation ofφ transforms spatial vectors expressed inframe G into ones expressed in a different frame, F:

φ(x,y) =

(

I3F˜l (x,y)

03 I3

)(

FRG 03

03FRG

)

=

(

FRGF˜l (x,y) FRG

03FRG

)

(1.31)

This coordinate-frame representation includes rotation matrices, and is convenientwhen chaining together rigid-body transformations across a sequence of frames. Inthis context, the x and y points are normally the F and G frames, respectively, i.e.,φ(x,y) ≡ φ(F,G).

The following lemma establishes important group properties of φ(·).Lemma 1.1 Group properties of φ(·).The following group properties hold for the φ(x,y) transformation matrix:

φ(x,x) = I6

φ(x,z) = φ(x,y)φ(y,z)

φ−1(x,y) = φ(y,x)

(1.32)

Proof: The first equation in (1.32) follows from (1.29) and uses the l(x,x) = 0identity.


To verify the second equation, observe that l(x,z) = l(x,y)+ l(y,z). Thus,

φ(x,y)φ(y,z) =

(

I3 ˜l(x,y)

03 I3

)(

I3 ˜l(y,z)

03 I3

)

=

(

I3˜l(x,y)+˜l(y,z)

03 I3

)

=

(

I3˜l(x,z)

03 I3

)

= φ(x,z)

The third equation follows by setting z = x in the second identity to obtain φ(x,y)φ(y,x) = φ(x,x) = I.

1.4.1 Spatial Velocity Transformations

Using (1.20) and (1.29), the spatial velocity relationship in (1.15) and (1.16) for apair of points x and y on a rigid body can be rephrased as:

V(y) = φ∗(x,y)V(x) (1.33)

The above uses the following fact:

φ∗(x,y) =

(

I3 ˜l(x,y)

03 I3

)∗=

(

I3 03˜l∗(x,y) I3

)

=

(

I3 03

−˜l(x,y) I3

)

The more compact (1.33) encompasses the information in the (1.15) and (1.16) pairof equations. Equation (1.33) shows that φ∗(x,y) relates the spatial velocity at x tothe spatial velocity at y on the body. This relationship thus, allows us to obtain thespatial velocity at any point on the rigid body from the spatial velocity of a singlepoint on the body.

1.4.2 Properties of φ(·)

The following exercise establishes some basic properties of φ(x,y) and spatial vec-tor operators.

Exercise 1.5 Identities involving ˜(·), (·) and φ(·, ·).For a spatial vector X, and a pair of frames x and y, prove the following identities:

[φ(x,y) X] = φ(x,y) Xφ∗(x,y)

[φ∗(x,y) X]

∼= φ∗

(x,y) ˜X φ−∗(x,y)

[φ∗(x,y) X]

∼φ∗

(x,y) = φ∗(x,y) ˜X

(1.34)

1.4 The Rigid Body Transformation Matrix φ(x,y) 13

The second identity in (1.34) parallels the well known identity Rl(x,y) =

R˜l(x,y)R∗, where R is a rotation matrix and l is a 3-vector.

Exercise 1.6 Rigid body transformation of ˜V(x).For two points, x and y on a rigid body, show that

φ∗(x,y)˜V(x) = ˜V(y)φ∗

(x,y)

V(x) φ(x,y) = φ(x,y)V(y)(1.35)

Exercise 1.7 Relationships involving Xω and Xv.For spatial vectors, X and Y, show that

XvYω

= ˜XvYv= 0 (1.36a)

˜Xv Y = ˜Xv Yω (1.36b)

XvX = X

vXv

= 0 (1.36c)

X X = ˜Xω X= ˜Xω Xv (1.36d)

φ(x,y)Xv

= Xvφ(x,y) = X

v(1.36e)

Exercise 1.8 The inertial frame derivative of φ(x,y).For an arbitrary pair of frames x and y, derive the following expressions for theinertial frame derivative of φ(x,y)

dIφ(x,y)dt

= Vv(y)− V

v(x) (1.37)

where V(x) and V(y) denote spatial velocities with respect to the inertial frameof x and y, respectively. Observe that this inertial derivative is independent of theangular velocity of the frames. When x and y are points on a rigid body, (1.37) canbe equivalently stated as

dIφ(x,y)dt

= V(y)− V(x) (1.38)

Exercise 1.9 Local time derivative of φ∗(x,y).This exercise examines the relationships between time derivatives with respect toframes that are translating and rotating with respect to each other.


1. Let F and G denote a pair of frames (not necessarily on a rigid body) withω(F,G) denoting the angular velocity of frame G frame with respect to frame F.Show that

VF(F,G) = φ∗(F,G)VG(F,G) (1.39)

where

VF(F,G)�=

[

ω(F,G)

vF(F,G)

]

and VG(F,G)�=

[

ω(F,G)

vG(F,G)

]

vF(F,G)�=

dFl(F,G)

dtand vG(F,G)

�=

dGl(F,G)

dt

(1.40)

Observe that VF(F,G) and VG(F,G) are both zero when F and G are frames onthe same rigid body.

2. With the coordinate representation of φ∗(F,G) defined by (1.31), we have:

φ∗(F,G) =

(

GRF 03

03GRF

)(

I3 03

−F˜l (F,G) I3

)

(1.41)

Show that the time-derivative of this coordinate representation of φ∗(F,G) isgiven by:

dφ∗(F,G)

dt= ˜VF(G,F)φ∗

(F,G) = φ∗(F,G)˜VG(G,F) (1.42)

3. Applying (1.42) to φ∗(G,F) it follows that

dφ∗(G,F)

dt= ˜VG(F,G)φ∗

(G,F) = φ∗(G,F)˜VF(F,G) (1.43)

On the other hand, since φ∗(G,F) = φ−∗(F,G), it follows from the matrix in-verse derivative identity in (A.28) on page 402 that

dφ∗(G,F)

dt= −φ∗

(G,F)dφ∗(F,G)

dtφ∗

(G,F) (1.44)

Verify that the expressions in (1.42) and (1.43) satisfy the identity in (1.44).4. For frames F, G and H, we have φ∗(F,H) = φ∗(G,H)φ∗(F,G). Show that the

time derivative expression in (1.42) satisfies the product rule for derivatives:

dφ∗(F,H)

dt=

dφ∗(G,H)

dtφ∗

(F,G)+φ∗(G,H)

dφ∗(F,G)

dt(1.45)

1.5 Spatial Forces 15

1.5 Spatial Forces

Define the spatial force spatial vector, f(x), at a frame x as

f(x)�=

[

N(x)

F(x)

]

(1.46)

where theN(x) denotes the rotational moment component and F(x) the linear forcecomponent.

We now look at the relationship between spatial forces at a pair of points x andy on a rigid body. The input power generated by a spatial force f is independent ofthe body reference point x or y. Thus

f∗(x)V(x) = f∗(y)V(y)

Using V(y)1.33= φ∗(x,y)V(x) in the above we obtain:

f∗(x)V(x) = f

∗(y)φ∗

(x,y)V(x)

Since V(x) can be chosen arbitrarily, it follows that the following relationship musthold:

f(x) = φ(x,y)f(y) (1.47)

This implies that the f(y) spatial force at a point y on a rigid body is equivalent toa spatial force f(x) = φ(x,y)f(y) at another point x on the body. The transforma-tion relationships in (1.47) and (1.33) are the duals of each other, and the φ∗(x,y)transformation for spatial velocities is the adjoint of the φ(x,y) transformation forspatial forces.

Chapter 2Single Rigid Body Dynamics

This chapter develops the dynamical equations of motion for a single rigid bodyin multiple different ways: about the body center of mass, about an arbitrary pointon the body, using body and inertial frame derivatives, and finally using an iner-tially fixed velocity reference point. The properties of each of these formulationsare explored. These alternative formulations also help illustrate the analytical andtransformation properties of spatial quantities.

2.1 Spatial Inertia and Momentum of a Rigid Body

We begin first by defining the spatial inertia and the spatial momentum of a rigidbody, and their relationships for points on the rigid body.

2.1.1 The Spatial Inertia

The kinetic energy of a single rigid body can be expressed as the following volumeintegral:

Ke =12

∫Ωv∗(x)ρ(x)v(x)dϑ(x) (2.1)

Here v(x) is the linear velocity of point x, ρ(x) the mass density at x, dϑ(x) is aninfinitesimal volume element containing x, and Ω is the body volume. The linearvelocity v(x) and the spatial velocity V(x) at the same point x are related by

v(x)1.20= [0, I]V(x) (2.2)

Substitution of (2.2) in (2.1) leads to the following new expression for the kineticenergy:


18 2 Single Rigid Body Dynamics

Ke =12

∫Ω

V∗(x)

[

0

I

]

ρ(x)[0, I]V(x)dϑ(x) (2.3)

Now pick a point k on the rigid body and let V(k) denote the spatial velocity at thatpoint. The spatial velocity at any other point x on the rigid body, V(x), is related toV(k) via

V(x)1.33= φ∗

(k,x)V(k) ∀ x ∈Ω (2.4)

This equation states that the spatial velocity V(x) at any point x on a rigid body canbe obtained from V(k) by making use of theφ∗(k,x) rigid body transformation ma-trix. Use of (2.4) in (2.3) allows us to re-express the kinetic energy of the body as

Ke2.3,2.4=

12V∗

(k)

{∫Ωφ(k,x)

[

0

I

]

ρ(x)[

0, I]

φ∗(k,x)dϑ(x)

}V(k)

=12V∗

(k)M(k)V(k) (2.5)

M(k) is the spatial inertia of the body about the point k defined by the inner termin the integral as follows:

M(k)�=

∫Ωφ(k,x)

[

0

I

]

ρ(x)[

0, I]

φ∗(k,x)dϑ(x)

=

∫Ω

(

I ˜l(k,x)

0 I

)[

0

I

]

ρ(x)[

0, I]

(

I 0

−˜l(k,x) I

)

dϑ(x)

=

∫Ω

(

−˜l(k,x)˜l(k,x) ˜l(k,x)

−˜l(k,x) I

)

ρ(x)dϑ(x) (2.6)

In other words,

M(k) =

(

J (k) m p(k)

−m p(k) mI3

)

∈R6×6 (2.7)

where

m�=

∫Ωρ(x)dϑ(x) ∈R1

p(k)�=

1m

∫Ω

l(k,x)ρ(x)dϑ(x) ∈R3

J (k)�= −

∫Ω

˜l(k,x)˜l(k,x)ρ(x)dϑ(x) ∈R3×3

(2.8)

2.1 Spatial Inertia and Momentum of a Rigid Body 19

J (k) represents the rotational inertia of the body about the point k; m is themass of the body; and p(k) is a 3-vector. Often, m, mp(k) and J (k) are referredto as the zeroth, first and second order moments of inertia, respectively of thebody about the point k.

Exercise 2.1 Rigid body center of mass.For two points x and y on a rigid body, show that the first moment vectors, p(x) andp(y), respectively, are related by

p(x) = l(x,y)+p(y) (2.9)

A consequence of this observation is that the point C, referred to as the centerof mass or centroid of the body, and defined by the first moment vector, p(k), isthe same point defined by the first moment vector, p(x) for the different referencepoint x. Thus, the location of the center of mass, is unique and independent of thechoice of reference point. From the definition of the center of mass, it is clear thatp(C) = 0, and therefore the M(C) spatial inertia about the center of mass has thesimpler block-diagonal form

M(C) =

(

J (C) 0

0 mI3

)

(2.10)

Except for the mass, the first and second moments of inertia, as well as the spatialinertia itself depend on the choice of the reference point.

Exercise 2.2 Parallel-axis theorem for rotational inertias.

1. For an arbitrary point x, show that the rotational moment of inertia J (x) isrelated to J (C) as follows:

J (x) = J (C)−m p(x) p(x) (2.11)

Equation (2.11) is referred to as the parallel-axis theorem for computing therotational inertia of a rigid body at point other than the center of mass from therotational inertia at the center of mass.

2. Show that J (x) is symmetric and positive semi-definite for all x. Also, show thatits minimum value is at the center of mass of the body. That is, J (x) � J (C)

for all points x.3. J (x) fails to be positive definite only for “pathological” rigid bodies. Give

examples of rigid bodies for whom J (x) fails to be positive definite.

A coordinate-frame representation of M(z) is a 6×6 symmetric, positive semi-definite matrix. While, in general, 21 elements are required to specify a 6 × 6symmetric matrix, only ten elements are required to specify a spatial inertia matrix


because of its special structure: one for the mass scalar, three for the center of masslocation 3-vector, and six for the symmetric rotational inertia matrix.

2.1.2 The Parallel-Axis Theorem for Spatial Inertias

Let us now examine the relationship between the spatial inertias, M(x) and M(y),about a pair of points x and y. From (2.6) we have

M(x) =

∫Ωφ(x,z)

[

0I

]

ρ(z)[0, I]φ∗(x,z)dϑ(z)

1.32=

∫Ωφ(x,y)

[

φ(y,z)

[

0I

]

ρ(z)[0, I]φ∗(y,z)

]

φ∗(x,y)dϑ(z)

= φ(x,y)

[∫Ωφ(y,z)

[

0

I

]

ρ(z)[0, I]φ∗(y,z)dϑ(z)

]

φ∗(x,y)

2.6= φ(x,y)M(y)φ∗

(x,y)

Thus, the spatial inertia M(x) about a point x is related to the spatial inertia M(y)

about another point y by the following expression:

M(x) = φ(x,y)M(y)φ∗(x,y) (2.12)

Equation (2.12) is the parallel axis theorem for spatial inertias and defines therelationship between the spatial inertias about arbitrary pairs of points on a rigidbody. Observe that neither of the points in (2.12) has to be the center of mass C.Using (2.11) (or alternatively by expanding out both sides of (2.12)) we obtain thefollowing expression relating the rotational inertias about the points x and y:

J (x) = J (y)−m p(x) p(x)+m p(y) p(y) (2.13)

The parallel axis theorem for spatial inertias is more compact when compared withthe parallel axis theorem for rotational inertias. It encapsulates (2.9) and (2.13) intoa single equation. While the concise form of (2.12) is analytically convenient, thecomponent level expressions in (2.9) and (2.13) are utilized for efficient computa-tional implementation of expressions involving the spatial inertia matrix.

Exercise 2.3 Positive semi-definiteness of spatial inertias.Show that M(x) is always positive semi-definite. Show that it is positive definite ifand only if J (C) is positive definite.

2.1 Spatial Inertia and Momentum of a Rigid Body 21

2.1.3 Spatial Inertia of a Composite Assemblage of Rigid Bodies

Equation (2.12) provides a way to compute the total spatial inertia of a body madeup of several component rigid bodies. For example, if a body is composed of a rigidbody with spatial inertia M1(x) about point x, rigidly attached to a second rigidbody with spatial inertiaM2(y) about point y, then the overall spatial inertia of thecomposite bodyM(z) about another point z is given by

M(z) = φ(z,x)M1(x)φ∗(z,x)+φ(z,y)M2(y)φ

∗(z,y)

In other words, the process of computing the overall spatial inertia of a body con-sists of first using the parallel-axis theorem to transform all the component spatialinertias to a common reference point, and then adding them up to obtain the overallspatial inertia. This process is necessary because the addition of spatial inertias ispermissible only for spatial inertias referenced about the same point.

Exercise 2.4 Invariance of the kinetic energy.Show that the kinetic energy of a rigid body defined by (2.5) does not depend on thereference point chosen for its computation. That is, for an arbitrary pair of points xand y on a rigid body, we have

Ke =12V∗

(x)M(x)V(x) =12V∗

(y)M(y)V(y) (2.14)

2.1.4 The Spatial Momentum of a Rigid Body

Let us consider a rigid body rotating with angular velocityω, and center of mass lin-ear velocity, v(C). About its center of mass, mv(C) represents the linear momentumof the body, while J (C)ω represents its angular momentum. We combine these todefine the spatial momentum of the body about its center of mass, h(C), as

h(C)�=

[

J (C)ω(C)

mv(C)

]

2.10= M(C)V(C) (2.15)

Generalizing the expression h(C) = M(C)V(C) to arbitrary points, the spatialmomentum of a rigid body about an arbitrary point z, h(z), is defined as

h(z)�= M(z)V(z) ∈R6 (2.16)


Since M(z) and V(z) depend on the reference point z, the h(z) spatial momentumdoes as well. The following exercise examines the relationship between the spatialmomenta about points on a rigid body.

Exercise 2.5 Relationship of spatial momenta about points x and y.Show that the h(x), spatial momentum of a rigid body about a point x, is relatedto the h(y) spatial momentum about another point y on the body, by the followingexpression:

h(x) = φ(x,y)h(y) (2.17)

Equation (2.17) states that we can obtain the spatial momentum of a rigid bodyabout any point on the body from the knowledge of the spatial momentum about aspecific point on the body.

So far we have encountered several instances of transformations that relate spatialproperties of a rigid body across different points on the body. Table 2.1 summarizessome of the key relationships involving the spatial quantities for a pair of points xand y on a rigid body.

Table 2.1 Relationship between rigid body quantities for a pair of points x and y on a rigid body.

Point y Point x

Spatial velocities V(y) = φ∗(x,y)V(x) V(x)

Spatial forces f(y) f(x) = φ(x,y)f(y)

Spatial inertias M(y) M(x) = φ(x,y)M(y)φ∗(x,y)

Spatial momenta h(y) h(x) = φ(x,y)h(y)

Kinetic energy 12 V∗(y)M(y)V(y) 1

2 V∗(x)M(x)V(x)

The central role played by φ(x,y) in these transformations is the reason for re-ferring to it as the rigid body transformation matrix.

2.2 Motion Coordinates

The motion state of a rigid body is completely characterized by its position and at-titude configuration and its linear and angular velocity coordinates. More generally,the motion state of any mechanical system is defined by its configuration and veloc-ity coordinates. The dynamics problem addresses the properties and mathematicalanalysis of the evolution of the motion state of a system with time. A key step in thedevelopment of dynamics models is the determination of the functional dependency

2.2 Motion Coordinates 23

of the time derivative of the motion state on the motion state itself, and the externalforces being applied on the system. This mathematical relationship is referred toas the equations of motion for the system. More precisely, the part of the equa-tions of motion pertaining to the time derivatives of configuration coordinates arereferred to as the kinematics part. The remaining part describing the derivatives ofthe velocities is referred to as the dynamics part. While the kinematics and dynam-ics differential equations are typically coupled, specific choices of coordinates canhelp decouple and simplify their expressions.

2.2.1 Generalized Coordinates and Velocities

The coordinates used to define the configuration state of a system are referred toas the generalized coordinates for the system. We use θ to denote the vector ofgeneralized configuration coordinates for a system. The dimensionality of θ is atleast as large as the number of velocity degrees of freedom in the system.

For a rigid body, the positional part of the generalized coordinates is typically justthe components of the position vector of the body frame with respect to the inertialcoordinate frame. The remaining generalized coordinates define the attitude of thebody. Appendix B describes some of the possible attitude generalized coordinateschoices for a rigid body. Options include Euler angles, angle-axis parameters andquaternion attitude representations.

The variables for parametrizing the velocity space of the system are referred to asthe generalized velocities for the system. We use β to denote the vector of general-ized velocity coordinates for a system. While the time derivatives of the generalizedcoordinates, θ, are typically chosen as generalized velocities, this is not required –and at times not the best choice. The generalized velocities are not even required tobe integrable quantities. Non-integrable velocity coordinates are also referred to asquasi-velocities, The term quasi-velocities designates pseudo-derivative since thevelocity coordinates are not the time derivatives of any function of the generalizedcoordinates.

For a rigid body, an obvious choices of generalized velocity coordinates are thecomponents of the spatial velocity, V(x), of a point x on the body represented eitherin the body frame, B, or in the I inertial frame. For convenience, the point x is oftenchosen as the location of the body frame B itself. For the B frame representation, thegeneralized velocity coordinates are denotedβB = BV(x), while for the I frame rep-resentation, the generalized velocities are denoted βI = IV(x). The angular velocitycomponents of the generalized velocities are examples of non-integrable velocitycoordinates.


2.2.2 Generalized Forces

The definition of the generalized forces for a system depends on the choice of gen-eralized velocities for the system. With β and T denoting the vectors of generalizedvelocities and forces, respectively for the system, the generalized forces must satisfythe requirement that the T∗β expression represents the input power. Since the inputpower is independent of the specific choice of generalized velocities, this require-ment implies that, if a new set of generalized velocities γ is chosen, the definition ofthe generalized forces must be changed accordingly. Thus, if γ is related to β by therelationship γ=A(θ)β, then it requires that A−∗(θ)T be the corresponding gener-alized forces. It is important to remember that the generalized forces do not have todirectly correspond to physical forces on the system, and can be a transformation ofthese forces.

2.2.3 Generalized Accelerations

The generalized acceleration coordinates are defined as the time-derivatives ofthe generalized velocity coordinates. Different choices for the generalized velocitycoordinates lead to different generalized acceleration coordinates. The generalizedaccelerations corresponding to the two choices of rigid body generalized velocitieswe have discussed are:

βB(x)�=

dBV(x)

dt=

dBV(x)

dtand βI(x)

�=

dIV(x)

dt=

dIV(x)

dt(2.18)

The relationship between βB(x) and βI(x) is as follows:

βI(x)1.28,2.18

= βB(x)+ ˜Vω(x)V(x)

1.36d= βB(x)+ V(x)V(x)

= βB(x)+

[

0

ω(x)v(x)

]

(2.19)

Equation (2.19) defines the relationship between the two definitions of generalizedaccelerations. As we will see in the following sections, different choices for thegeneralized velocities lead to different formulations of the equations of motion. Un-doubtedly, since these different formulations refer to the same physical system, theyare fundamentally equivalent descriptions of the system and can be transformed intoeach other.

2.3 Equations of Motion with Inertial Frame Derivatives 25

2.3 Equations of Motion with Inertial Frame Derivatives

In this section we formulate the equations of motion of a rigid body using inertialframe derivatives for generalized velocities.

2.3.1 Equations of Motion with βI = IV(C)

It is well known that the linear and angular equations of motion of a rigid bodyare decoupled from each other about the center of mass C. With N(C) and F(C)

denoting the moment and linear force at the center of mass of the body, the dynam-ical equations of motion for a rigid body are defined by Newton’s law and Euler’sequations [185] as follows:

N(C) =dIJ (C)ω(C)

dtand F(C) =

dImv(C)

dt(2.20)

Equation (2.20) states that about the center of mass, the rate of change of the angularand linear momentum of the rigid body are equal to the applied moment and force,

respectively. In other words, with the spatial force at C denoted as f(C)�=

[

N(C)

F(C)

]

,

and using (2.15), (2.20) can be rephrased as

f(C) =dIM(C)V(C)

dt=

dIh(C)

dt(2.21)

This more compact expression states that the rate of change of the inertial frame spa-tial momentum about the center of mass for the body, h(C), is equal to the externalspatial force at its center of mass.

It follows that in the absence of external spatial forces, i.e., f(C) ≡ 0, the spa-tial momentum vector h(C) remains constant with time in the inertial frame. Thisproperty is also known as the conservation of spatial momentum property for a rigidbody in the absence of external forces.

The following exercise derives an expression for the inertial time derivative ofthe spatial inertia of a rigid body about an arbitrary point on the body. We will usethis later to establish the equations of motion for the body.

Exercise 2.6 Time derivative of rigid body spatial inertia.Let z denote an arbitrary point on a rigid body. Since the spatial inertia of a rigidbody is constant, the body frame time derivative of the spatial inertia matrix M(z)

is zero. Show that the inertial frame time derivative, M(z), is

M(z)�=

dIM(z)

dt= ˜Vω

(z)M(z)−M(z)˜Vω(z) (2.22)


where ˜Vω(z) contains the angular component of the spatial velocity V(z) as definedin (1.21) on page 9.

The following lemma summarizes the equations of motion for a rigid body usingIV(C) as the generalized velocities.

Lemma 2.1 Body equations of motion using βI = IV(C).The equations of motion for a rigid body about its center of mass using inertialframe derivatives are as follows:

f(C) =M(C)βI(C)+bI(C)

where bI(C)�= ˜Vω

(C)M(C)Vω(C)

= Vω

(C)M(C)Vω(C) =

[

ωJ (C)ω

0

]

(2.23)

Proof: We have

f(C)2.21= M(C)βI(C)+

dIM(C)

dtV(C)

2.22= M(C)βI(C)+

[

˜Vω(C)M(C)−M(C)˜Vω

(C)

]

V(C)

1.22= M(C)βI(C)+ ˜Vω

(C)M(C)Vω(C)

+

[

˜Vω(C)M(C)Vv

(C)−M(C)˜Vω(C)Vv

(C)

]

2.10= M(C)βI(C)+ ˜Vω

(C)M(C)Vω(C)

The last step used the expression forM(C) from 2.10 to verify that

[

˜Vω(C)M(C)Vv

(C)−M(C)˜Vω(C)Vv

(C)

]

= 0

The last equality in (2.23) follows from expanding out ˜Vω(C)M(C)Vω(C).

The following exercise establishes that the bI(C) gyroscopic spatial force is anon-working spatial force.

Exercise 2.7 bI(C) gyroscopic spatial force does no work.The power generated by a spatial force at a point on a rigid body is its dot productwith the spatial velocity at the point. Show that the bI(C) gyroscopic spatial forceterm does no work, i.e., V∗(C)bI(C) = 0.

2.3 Equations of Motion with Inertial Frame Derivatives 27

2.3.2 Equations of Motion with βI = IV(z)

The previous section used the spatial velocity at the C center of mass, IV(C), asthe generalized velocity coordinates for the rigid body. In this section we switch tousing the spatial velocity, IV(z), at an arbitrary point z on the body as the general-ized velocity coordinates for the body. First, the following exercise establishes therelationship between the generalized accelerations βI between different points on arigid body.

Exercise 2.8 Inertial generalized accelerations at two points.Show that the “inertial” generalized accelerations, βI(x) and βI(y), at a pair ofpoints x and y on a rigid body are related to each other as follows:

βI(y)�=

dIV(y)

dt= φ∗

(x,y)βI(x)− ˜V(y)V(x)

= φ∗(x,y)βI(x)+

[

0

ωω l(x,y)

]

(2.24)

For an arbitrary point z on the rigid body, l(z,C) is the first moment vector p(z).Thus, from (2.24) it follows that:

βI(z)�=

dIV(z)

dt= φ∗

(C,z)βI(C)+

[

0

−ωω p(z)

]

(2.25)

The equations of motion of a rigid body about an arbitrary point z using inertialframe derivatives are summarized in the following lemma.

Lemma 2.2 Body equations of motion with βI = IV(z).The equations of motion for a rigid body about an arbitrary point, z, using inertialframe derivatives are:

f(z) =M(z)βI(z)+bI(z)

where bI(z)�= ˜Vω

(z)M(z)Vω(z) = V

ω(z)M(z)Vω

(z)

=

[

ωJ (z)ω

mωω p(z)

]

(2.26)


Proof: We have,

M(z)βI(z)2.25= M(z)

(

φ∗(C,z)βI(C)+

[

0

−ωω p(z)

])

2.12,2.7= φ(z,C)M(C)βI(C)−

[

m p(z)ωω p(z)

mωω p(z)

]

2.23,A.1= φ(z,C)

(

f(C)−

[

ωJ (C)ω

0

])

−

[

−mω p(z) p(z)ω

mωω p(z)

]

1.47= f(z)−

[

ω(

J (C)−m p(z) p(z))

ω

mωω p(z)

]

2.11= f(z)−

[

ωJ (z)ω

mωω p(z)

]

This establishes the latter equality in (2.26). The middle equality follows from thefollowing:

˜Vω(z)M(z)Vω

(z) =

(

ω 0

0 ω

)[

J (C)ω

−m p(z)ω

]

=

[

ωJ (z)ω

mωω p(z)

]

The more general equations of motion in (2.24) reduce to the ones about thecenter of mass C when z = C since p(C) = 0. Observe that the expression for thegyroscopic spatial force in (2.26) depends only on the angular velocity componentand is independent of the the linear velocity. Thus, the instantaneous generalized ac-celeration in this formulation is independent of the linear velocity of the body. Thisformulation is therefore, less susceptible to numeric integration errors for systemswith large linear velocities (e.g. space vehicles).

Exercise 2.9 bI(z) gyroscopic spatial force does work.Show that the bI(z) gyroscopic force term does work. On the other hand, show thatas is to be expected, the kinetic energy is conserved in the absence of external forceswhen using the inertial derivative form of the equations of motion.

Exercise 2.10 Non-conservation of spatial momentum.In (2.21) we observed that the rate of change of the spatial momentum about thecenter of mass is equal to the spatial force at the center of mass. In contrast, showthat this equality does not hold for other reference points, z, that is

f(z) �= dIh(z)

dt(2.27)

when the point z is not the center of mass.

2.4 Equations of Motion with Body Frame Derivatives 29

One consequence of (2.27) is that the spatial momentum h(z) vector is notconstant in the inertial frame even in the absence of external forces, i.e., even whenf(z) ≡ 0 when the point z is not the center of mass of the body. In other words, thespatial momentum is not conserved in the absence of external forces at points otherthan the center of mass.

2.4 Equations of Motion with Body Frame Derivatives

In this section we switch gears to examine the equations of motion of a rigid bodyusing body frame derivatives for the generalized velocities.

Lemma 2.3 Body equations of motion with βB = BV(z).The equations of motion for a rigid body about an arbitrary point z using body framederivatives are:

f(z) =M(z)βB(z)+b(z)

where b(z)�= V(z)h(z) = V(z)M(z)V(z) (2.28)

Proof: Using (2.19) we have

M(z)βB(z)2.19= M(z)

(

βI(z)−

[

0

ω v(z)

])

2.26,2.7= f(z)−

[

ωJ (z)ω

mωω p(z)

]

−

[

m p(z)ω v(z)

mω v(z)

]

= f(z)−

[

ωJ (z)ω+m p(z)ω v(z)

mωω p(z)+mω v(z)

]

A.1= f(z)−

[

ωJ (z)ω−m(

ω v(z)p(z)+ v(z) p(z)ω)

mω(

ω p(z)+v(z))

]

= f(z)−

[

ω(

J (z)ω+m p(z)v(z))

−m v(z) p(z)ω

mω(

ω p(z)+v(z))

]

= f(z)− V(z)

[

J (z)ω+m p(z)v(z)

−m p(z)ω+mv(z)

]

2.7= f(z)− V(z)M(z)V(z)

2.16= f(z)− V(z)h(z)

In contrast with the inertial derivative formulations, the expression for thegyroscopic spatial force in (2.28) depends on the linear velocity of the body.


Thus, this formulation is more susceptible to numeric errors when the linear veloci-ties are large.

Remark 2.1 Gyroscopic force b(z) does no work.We show now that the gyroscopic spatial force b(z) in the equations of motion inLemma 2.3 does no work. The power generated by the gyroscopic force b(z) is

V∗(z)b(z)

2.28= V∗

(z)V(z)h(z)1.24= 0 (2.29)

confirming that b(z) is a non-working force.That the gyroscopic spatial force does no work for equations of motion about

arbitrary points when body frame derivatives are used is in contrast with our ob-servation in Exercise 2.9 that this property only holds at the center of mass wheninertial frame derivatives are used.

Remark 2.2 Conservation of kinetic energy for a rigid body.We now show that the equations of motion in Lemma 2.3 further imply that thekinetic energy of a body is conserved in the absence of external forces, i.e., f(z)≡ 0.The time derivative of the kinetic energy is given by

12

dV∗(z)M(z)V(z)

dt= V∗

(z)M(z)βB(z)2.28= −V∗

(z)b(z)2.29= 0

This implies that the kinetic energy of a body is conserved when the external forceson the body are zero.

Exercise 2.11 Equations of motion using spatial momentum.Using the spatial momentum derivative expression in (2.21) as a starting point,derive the equations of motion in (2.28).

Remark 2.3 Body dynamics invariance across Newtonian frames.Newtonian frames are free-falling frames. A frame moving at fixed linear velocitywith respect to a Newtonian frame is also a Newtonian frame. Frames that are accel-erating or rotating with respect to a Newtonian frame are not Newtonian frames. Ingeneral, the dynamics of a multibody system are invariant across Newtonian framesbut not across non-Newtonian ones.

We examine the implications of this invariance property for the single rigidbody system that we have studied so far. We should expect that formulations us-ing derivatives with respect to Newtonian frames (e.g. inertial frame derivatives)would have equations of motion that are independent of the linear velocity of thebody. On the other hand we can expect that formulations using derivatives in thenon-Newtonian frames (e.g. rotating body frames) would not necessarily retain thisinvariance property.

2.5 Equations of Motion with an Inertially Fixed Velocity Reference Frame 31

These conjectures can be verified by examining the expression for the bI(z) spa-tial force in (2.26) for the inertial frame derivative dynamics, and noting that it isindependent of the linear velocity. On the other hand, the corresponding gyroscopicspatial force, b(z) in (2.28) for body frame derivatives does depend on the linearvelocity of the body.

2.5 Equations of Motion with an Inertially Fixed VelocityReference Frame

In the previous sections we have used the spatial velocity of a physical point on therigid body as the generalized velocity coordinates of a rigid body to derive its equa-tions of motion. However, there is another interesting option that uses the effectivespatial velocity of an inertially fixed reference point as the generalized velocities for

I

C

z

Fig. 2.1 A rigid body with an inertially fixed velocity reference point

the body. Without loss in generality, we denote the inertial frame I as this velocityreference point. The inertially referenced spatial velocity, VI, is defined as thespatial velocity of the velocity reference I viewed as being instantaneously attachedto the rigid body (see Fig. 2.1). Thus, VI is given by the expression

VI

�=

[

ω

vI

]

�= φ∗

(C,I)V(C) (2.30)

with ω and vI denoting the angular and linear velocity components, respectively.The exercise below shows that the use of the center of mass C as the point of refer-ence for defining VI in (2.30) is in fact arbitrary, in the sense that any other point onthe body would yield the same value for VI.


Exercise 2.12 Invariance of VI to velocity reference point.Equation (2.30) used the spatial velocity at the center of mass to obtain the value ofVI. Show that the value of VI is the same if a different point on the rigid body was

used instead. That is, show that for any other point x on the rigid body

VI = φ∗(x,I)V(x) (2.31)

Due to the invariance with respect to the reference point, we use the terminol-ogy VI instead of VI(C) for the inertially referenced spatial velocity of a body. Inessence, the use of an inertially fixed spatial velocity reduces all the points on therigid body to an equivalence class. We now use the inertial frame representation ofVI as the generalized velocities to derive the equations of motion for a rigid body.

The spatial inertia, MI, of the body about the I reference frame is given by

MI

�= φ(I,C)M(C)φ∗

(I,C) =

(

JI m pI

−m pI mI3

)

(2.32)

pI = l(I,C) denotes the 3-vector from the inertial frame to the location of the centerof mass of the body.

While Exercise 2.6 on page 25 derives the expression for the time derivative ofthe M(z) body spatial inertia with respect to the inertial frame, the next exercisederives the corresponding expression for the time derivative of MI with respect tothe inertial frame.

Exercise 2.13 Time derivative of MI.Show that the inertial time derivative of MI is given by

MI

�=

dIMI

dt= VIMI −MI

˜VI = VIMI +MI

(

VI

)∗ (2.33)

The inertially referenced spatial momentum, hI, is defined as

hI

�= MIVI = φ(I,x)h(x) (2.34)

Similarly, the inertially referenced spatial force, fI, is defined as

fI�= φ(I,C)f(C) (2.35)

Exercise 2.14 Equations of motion about a fixed velocity referencepoint.

1. Show that the momentum form of the equations of motion about the inertiallyfixed velocity reference frame is given by:

2.6 Comparison of the Different Dynamics Formulations 33

fI =dIhI

dt(2.36)

This expression is similar to (2.21), where the generalized velocities were theinertial frame derivative of the center of mass spatial velocity. Equation (2.36)implies that the inertially referenced spatial momentum vector hI is constantwhen the external spatial force fI ≡ 0.

2. Define the generalized velocity coordinates as βI

�= IVI. Show that the equations

of motion using βI are given by

fI =MIβI +bI (2.37)

wherebI

�= MIVI = VIMIVI = VIhI (2.38)

Observe that the spatial vector expression for bI in (2.38) is identical in form tothe b(z) gyroscopic force expression for body fixed velocity reference frames in(2.28).

3. Verify that the gyroscopic term bI is non-working and that the kinetic energy ofthe body is indeed conserved when fI(t) ≡ 0.

2.6 Comparison of the Different Dynamics Formulations

Summarized here and in Table 2.2 are some of the properties for the alternative rigidbody dynamics formulations. the following conservation facts are true:

• The kinetic energy of the body is always conserved in the absence of externalforces (Remark 2.2).

• In the absence of external forces,

– The spatial momentum vector of a body about its center of mass remainsconstant in the inertial frame (2.21).

– The spatial momentum vector of a body about points on the rigid body otherthan the center of mass does not remain constant (Exercise 2.10).

– The spatial momentum vector about an inertially fixed velocity reference pointremains constant (Exercise 2.14).

• The gyroscopic force for the body does not do any work when the general-ized velocities are either body frame derivatives of the body spatial velocity(Remark 2.1) or are inertially referenced spatial velocities (Exercise 2.14). How-ever the body gyroscopic force does do work when the generalized velocitiesare inertial frame derivatives of the body spatial velocity about a non-C location(Exercise 2.9).


• The gyroscopic force terms are independent of the linear velocity of the bodywhen inertial derivatives are used for the generalized velocities. This indepen-dence does not hold for body frame derivatives or for the inertially referencespatial velocity based generalized velocities.

While analytically all of the formulations are essentially equivalent, the conclusionfrom the above comparison is the numeric and computational superiority of for-mulating the dynamics of a rigid body about its center of mass using inertial framederivatives for the generalized velocities. In practice, other considerations may drivethe choice of one of the alternate formulations. More discussion on rigid body dy-namics is available in [60, 69, 70, 96, 112, 124, 153, 185].

Table 2.2 A comparison of the properties of the different formulations for the dynamics of a singlerigid body

Formulation I deriv, C I deriv, z B deriv, z Inertial ref, I

Section 2.3.1 2.3.2 2.4 2.5

Gen. vel. β IV(C) IV(z) BV(z) VI

Gyroscopicforce b

Vω

(C)M(C)Vω(C) Vω

(z)M(z)Vω(z) V(z)h(z) VIhI

Conserved spa-tial momentum

√ √

Non-working b√ √ √

Independent oflinear velocity

√ √

Remark 2.4 Motion invariances for a rigid body.We have examined the conservation properties of the kinetic energy and spatial an-gular momentum of a body in the absence of external forces. These conservationproperties are a consequence of Noether’s theorem [11,60] which shows that cer-tain symmetries of the system Lagrangian of a system result in integrals of motion,i.e., quantities that are conserved with time.

For our rigid body example, the non-dependency of the kinetic energy of thebody on time results in the conservation of kinetic energy over time. Moreover, thenon-dependence of the kinetic energy on the location as well as the orientationof the rigid body results in the conservation of the linear and angular momentumproperties for the body.

Chapter 3Serial-Chain Kinematics

This chapter develops kinematics models for serial-chain rigid multibody systems.Although serial-chains are perhaps the simplest examples of multibody systems,many of the concepts and techniques developed for these systems apply, with onlymodest extensions, to general multibody systems.

3.1 Serial-Chain Model

A rigid multibody system consists of a collection of rigid bodies (also referred toas links) connected to each other via hinges (also referred to as joints). A hingeconnecting a pair of bodies defines the allowable relative motion between the bodies,and implicitly the constraints on the relative motion between the bodies. The hingeterminology is not restricted to just physical hinges, but can be used to representgeneral bilateral constraints between bodies.

Topologically, a serial-chain multibody system contains a single base-body anda single tip-body. Each internal body has one parent and one child body. The base-body has no parent body and the tip-body has no child body. The neighboring linkin the direction towards the base (tip) is referred to as the inboard (outboard) link.The serial-chain is assumed to have a total of n links (and thus, has n hinges).Without loss in generality, the links are assumed to be numbered from 1 through nfrom the tip to the base.1 The kth hinge connects the (k+1)th and the kth link, asillustrated in Fig. 3.1. Associated with the kth hinge are a pair of frames, Ok andO

+k , attached to the kth and (k+1)th links, respectively. Even though for the sake

of clarity Fig. 3.1 shows the O+k and Ok hinge frames at different locations, these

frames at any instant coincide with each other. The relative spatial velocity acrossthe kth hinge is defined as the relative spatial velocity of the Ok frame with respectto the O

+k frame. Associated with each body in the serial-chain is a body-fixed

1 Section 6.4 on page 111 studies important mathematical parallels between multibody dynamicsand estimation theory. The parallels are made more evident by this link numbering scheme.


36 3 Serial-Chain Kinematics

O+k+ 1

O+k

O+k− 1

Ok+ 1

Ok

Ok− 1

Bk+ 1

Bk+ 2

Bk

Bk− 1

p(k+1)p(k)

l(k+1,k)

l(k,k−1)kth

hinge

Towards Base Towards Tip

(k+1)th

hinge(k−1)th

hinge

kth link(k+1)th link

CC

Fig. 3.1 Illustration of links and joints in a serial rigid body system with body and hingeframe

reference frame. The body frame for the kth body is denoted Bk. In general, Bk

can be located anywhere on the kth body. Common location choices for Bk includethe center of mass of the link, the Ok frame or other convenient locations on thebody.

3.2 Hinge Kinematics

Hinges represent constraints between two bodies which restrict the relative con-figuration as well as motion between them. A noteworthy fact is that, while theconstraints on the relative configuration can be complex nonlinear equations, theconstraints on the relative velocity for virtually all known physical systems are inthe form of linear constraints. The constraints on the relative spatial velocity maybe expressed via a hinge map operator which specifies the allowable velocity rangespace in R6, or alternatively, via constraint equations on the relative spatial velocity.The former is an explicit characterization while the latter is an implicit characteri-zation of the hinge constraints. Typically, for physical hinges, the explicit character-ization is easy to obtain from the description of the hinge, while for hinges that area part of kinematic loops, the implicit characterization is often more readily avail-able. The two characterizations are equivalent, and either one can be chosen basedon convenience.

3.2 Hinge Kinematics 37

3.2.1 Hinge Generalized Coordinates

To simplify notation, we will frequently use “k” instead of Bk in expressions perti-nent to the serial-chain bodies. Thus, k+1

Tk will denote the Bk+1TBkhomogeneous

transform between the body frames of the (k+ 1)th and kth bodies. It has thegeneral form:

k+1Tk =

k+1TOk

·OkTk =

(

k+1T

O+k·O

+k TOk

)

·OkTk (3.1)

We can make the following observations:

• k+1TO

+k

is a pure translation transformation since the Bk+1 and O+k frame are

attached to the (k+ 1)th body. For hinges with translational components, thetranslation vector between these frames will vary with the hinge motion.

• OkTk is also a pure translation transformation that is constant, since the Ok andBk frames are rigidly attached to the kth body.

• O+k TOk

is a pure rotational transformation. For hinges with rotational compo-nents, the rotation value varies with the motion of the hinge.

The variation in k+1TOkwith changes in the hinge configuration can be param-

etrized by a set of variables. If the hinge has rp(k) configuration degrees of freedom,it is natural and quite common to use an rp(k)-dimensional vector to parametrize theconfiguration of the hinge. The elements of this vector, denoted θ(k), are referredto as the generalized coordinates for the hinge. rp(k) is the minimal number ofparameters that are required, and in most cases they are adequate for the requiredparametrization. However, there are situations where a larger number of parame-ters, i.e., a non-minimal parameter representation of the configuration of the hinge,is desirable, especially when dealing with the representation of orientations. It iswell-known that a minimal 3-parameter representation (such as the Euler angles)cannot provide a globally singularity-free representation for general orientations,and a 4-variable parametrization (such as the Euler parameters) is needed to meetthis requirement (see Wittenburg [185]). Thus, even when the physical hinge is well-behaved, the use of minimal representations may lead to undesirable numericallyill-conditioned behavior. Issues such as this may dictate the use of configurationvariables of dimension larger than the number of configuration degrees of freedom.

Even though most systems have single degree of freedom hinges (i.e., rp(k) = 1),there are important examples of hinges with rp(k)> 1. The specification of the θ(k)hinge generalized coordinates completely determines the configuration of the kthhinge. Thus, the full set of these hinge coordinates are referred to as the generalizedcoordinates for the full serial-chain.


3.2.2 Relative and Absolute Coordinates

The hinge generalized coordinates are also referred to as relative coordinates, sincethey specify the relative configuration of neighboring bodies. An alternative is theuse of absolute coordinates, which directly specify the configuration of each bodywith respect to the inertial frame. In contrast with relative coordinates, absolutecoordinates for each body are of dimension 6 or more, as seen in the discussion ofsingle body dynamics in Chap. 2.

Absolute coordinate formulations are popular in the dynamics simulation arena,due to the relative simplicity of such models, when compared with relative coordi-nate models [158]. However, relative coordinates offer distinct advantages that webelieve more than compensate for the added complexity. The minimal size of rela-tive coordinate representations, avoids over-parametrization related issues that arisefrom the use of absolute coordinates (e.g. drift error control, differential-algebraicequations). Moreover, the minimal size of relative coordinate representations pro-vides a natural, dynamical state representation, for robotics control system analysisand design.

We adopt relative coordinates in our study of multibody system dynamics. Theapparent added complexity of the relative coordinates system dynamics is easilyovercome with the right set of mathematical tools and concepts. In the forthcom-ing chapters, we develop several analytical techniques, that provide insight into thestructural properties of relative coordinate dynamics models. In addition, we derivesignificantly more efficient, and numerically superior, computational algorithms us-ing the relative coordinates approach.

3.2.3 Hinge Generalized Velocities

Since the motion of the bodies in a serial-chain are caused by hinge motions, we nowlook at formalizing the differential (i.e., velocity) kinematics for hinges. Towardsthis, differentiating k+1TOk

with respect to time yields:

dk+1TOk

dt1.3=

⎛

⎝

ω(O+k ,Ok) v(O+

k ,Ok)

0 0

⎞

⎠ (3.2)

where ω(O+k ,Ok) and v(O+

k ,Ok) denote the angular and linear velocities of theOk frame with respect to to the O

+k frame. Adopting the notation

Δω(k)�= ω(O

+k ,Ok) and Δv(k)

�= v(O+

k ,Ok) (3.3)


allows us to re-express (3.2) as

dk+1TOk

dt=

(

˜Δω(k) Δv(k)

0 0

)

(3.4)

The spatial velocity across the kth hinge, i.e., the spatial velocity of the Ok framewith respect to the O

+k frame is denoted ΔV(k) and is defined as

ΔV(k)�= V(Ok)−V(O

+k ) =

[

Δω(k)

Δv(k)

]

(3.5)

The dimensionality of the configuration dependent subspace (of R6) in which therelative spatial velocity lives defines the number of velocity degrees of freedom,rv(k), for the hinge. Though in most cases rp(k) = rv(k), in general

rp(k) � rv(k) (3.6)

When a hinge is simple with small number of degrees of freedom, it is a commonpractice to parametrize its velocity degrees of freedom using the time derivativesof the generalized coordinates. However, generalized velocity coordinates are moregenerally allowed to be non-integrable quasi-velocities. These can represent a moreconvenient choice for parametrizing the velocity degrees of freedom, and can de-couple and simplify the dynamical and kinematical equations of motion. A clas-sic example of the use of quasi-velocities is in the development of the dynamicalequations of motion for a free rigid body. The use of the time derivatives of theorientation parameters (such as Euler angles, Euler parameters, etc.) as part of the6-dimensional motion parameters leads to complicated equations of motion. How-ever, the use of the spatial velocity vector as the set of the 6-dimensional generalizedvelocities leads to much simpler equations of motion [124]. The time derivativesof the generalized coordinates are related to the generalized velocities via a bi-directional, possibly configuration dependent, map.

The relationship of the generalized velocities to the relative spatial velocityacross the hinge is usually quite straightforward. The configuration dependent ba-sis vectors for the hinge relative spatial velocity subspace are used as column vec-tors to define the configuration-dependent, full-rank hinge map matrix,2 denotedH∗(k) ∈ R6×rv(k), for the kth hinge. H∗(k) defines a linear mapping of the gen-eralized velocity coordinates vector, β(k) ∈ Rrv(k), for the hinge to the relativespatial velocity ΔV(k) across the hinge as follows:

ΔV(k) =H∗(k)β(k) (3.7)

In some instances, the above equation can have an additional bias term on the rightof the above equation. As discussed in more detail in Sect. 11.1 on page 209, thehinge constraint is said to be catastatic or acatastatic depending on whether thisterm is zero, or non-zero, respectively.

2 A hinge map matrix is also at times referred to as the joint map matrix.


SinceH∗(k), can be configuration dependent, it is possible for its rank to changeat certain configurations. When this happens, it is important to redefine it so that thehinge map matrix has maximal column rank again. Partitioned into the angular andlinear components hω(k) ∈ R3×rv and hv(k) ∈ R3×rv , H∗(k) can be expressedin the form

H∗(k) =

[

hω(k)

hv(k)

]

where Δω(k) = hω(k)β(k), Δv(k) = hv(k)β(k) (3.8)

In general the time derivatives of the generalized coordinate vector, θ(k) ∈Rrp(k),and the generalized velocity coordinates vector, β(k) ∈Rrv(k), are related to eachother by a configuration dependent linear transformation. However, in most casesβ(k) = θ(k), and in this case, rp(k) = rv(k) and the transform is the identity trans-formation.

Collectively, the β(k) generalized velocities for the hinges completely para-metrize the velocities of all the bodies in the multibody system. Hence, the full setof these hinge generalized velocities is also referred to as the generalized velocitiesfor the full serial-chain.

3.2.4 Examples of Hinges

Figure 3.2 shows examples of H∗(·) hinge map matrices for some common hingetypes.

⎛

⎜

⎜

⎜

⎜

⎝

001000

⎞

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎝

000001

⎞

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎝

00100p

⎞

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎝

0 00 01 00 00 00 1

⎞

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎝

1 0 00 1 00 0 10 0 00 0 00 0 0

⎞

⎟

⎟

⎟

⎟

⎠

rotary pin hinge prismatic hinge helical hinge cylindrical hinge spherical hinge1 dof 1 dof 1 dof 2 dof 3 dof

Fig. 3.2 Examples of hinge map matrices represented in the body frame

• The rotary pin hinge is a single degree of freedom hinge which only allows rota-tional motion across the hinge. The example in Fig. 3.2 allows motion about thez-axis.

• The prismatic example in the figure is also a 1 degree of freedom hinge, butallows only translational motion along the z-axis.


• The helical hinge allows 1 degree of freedom rotational and translational motion– but where the rotational and translational motions are coupled. The p term inits hinge map matrix represents the pitch, and denotes the ratio of translational torotational motion for the hinge.

• The cylindrical hinge is a 2 degree of freedom hinge, with independent 1 degreeof freedom rotational and translational motions about and along the z-axis.

• The last example in Fig. 3.2 is a spherical rotational hinge, where full 3 degreeof freedom rotational motion is allowed across the hinge. A popular choice forgeneralized coordinates for such hinges are Euler angle coordinates discussedin Sect. B.1 in the Appendix. While these coordinates have the virtue of be-ing of minimal size, their representation of the hinge configuration is not sin-gularity free. It is well known that representations of size 4 or more, such asthe unit quaternions discussed in Sect. B.3, are required for singularity-free rep-resentations. These latter choices for coordinates are examples of cases whererp(k)> rv(k).

One way of modeling a free-flying system is to assign a 6 degree of freedom hingebetween the base-body and the inertial frame. The H∗ hinge map matrix for thishinge is simply the 6×6 identity matrix.

Exercise 3.1 Hinge map matrix for a universal joint.Derive the configuration dependent hinge map matrix H∗ for a 2 degree of freedomuniversal joint whose two axes are parallel to the X and Y axes, respectively, in thezero configuration.

Unlike all the constant hinge map matrix examples in Fig. 3.2, the hinge mapmatrix for a universal joint is configuration dependent. However, the matrix dependsonly upon the first hinge degree of freedom coordinate, and not the second one.

Exercise 3.2 Hinge map matrix for a sphere rolling on a surface.Consider a spherical ball rolling without slipping on a level surface. The no slippagecondition requires that the linear velocity of the point on the ball that is instanta-neously in contact with the surface be zero. Let the reference frame for the ball belocated at its center. It is easy to see that there are 3 velocity degrees of freedombetween the ball and the surface. Determine the 6× 3 hinge map H∗ for the hingedefined by the contact between the ball and the surface.

In Exercise 3.2, there is only one holonomic constraint on the hinge which con-strains the center of the ball to remain a fixed distance from the floor. It precludesmotions of the ball in the vertical direction, i.e., no bouncing motion is allowed.The remaining constraints are examples of non-holonomic constraints.3 The ballpossesses five configuration degrees of freedom: two positional degrees of freedomwhich specify the location of the ball on the floor, and all three orientational de-grees of freedom for the ball. However, due to the non-slippage restriction, the ballhas only three velocity degrees of freedom: it can roll along the floor, and it can

3 Holonomic and non-holonomic constraints are defined more precisely in Sect. 11.1 on page 209.


twist about the vertical. It cannot move with a purely linear velocity on the floorsince sliding is not possible. In fact during the roll motion, the linear and angularvelocities are directly coupled together.

The point of contact between the ball and the surface changes as the ball moves.The hinge map matrix for this contact constraint is a constant matrix in the inertialframe because of the spherical nature of the ball. However, for non-symmetricalballs, the hinge map matrix will be configuration dependent. A more detailed dis-cussion of generalized hinges and constraints can be found in Schwertassek andSenger [161].

3.3 Serial-Chain Kinematics

The forward kinematics problem for multibody systems deals with the mathemat-ical mapping from the generalized coordinates and velocities of the system to thespatial location, orientation and velocities of the link frames. The inverse kine-matics problem focuses on the converse, i.e., the mathematical mapping from linkframe configuration and velocity data (usually end-effector frame in robotics) tothe corresponding generalized coordinate and velocities for the system. Differentialkinematics refers specifically to the velocity mappings.

3.3.1 Serial-Chain Configuration Kinematics

Let ITBk(or equivalently ITk) denote the homogeneous transform relating frame

Bk to the inertial frame I. The following recursive relationship relates the homoge-neous transforms for the kth and (k+ 1)th bodies:

ITk

1.5=

ITk+1 ·k+1

Tk (3.9)

This mathematical relationship for the configuration kinematics of the serial-chaindescribes how the inertial transforms for the component links are related to the inter-link transforms. Algorithm 3.1 describes a recursive algorithm for computing theITk transforms for all the links.

The location and orientation of any link frame with respect to another linkframe can be obtained by chaining together the component link-to-link ho-mogeneous transformations. Thus, with the kth link being outboard of thejth link, the transformation between these frames can be obtained using thefollowing product of link-to-link component transformations in a base-to-tiprecursion:

jTk

1.5=

jTj−1 · · ·k+1

Tk (3.10)

3.3 Serial-Chain Kinematics 43

Algorithm 3.1 Recursive computation of link transformations⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

ITn+1 = I

for k = n · · ·1ITk =

ITk+1 · k+1

Tk

end loop

3.3.2 Serial-Chain Differential Kinematics

For a serial-chain with known θ(k) generalized coordinates and θ(k) generalizedvelocities, the particular problem addressed here is the computation of the corre-sponding spatial velocities of the links. The spatial velocity of the kth link is definedas the spatial velocity of its body frame Bk, and denoted

V(k) ≡ V(Bk) (3.11)

For the sake of simpler exposition, and without losing any generality, we assumefor now that the body reference frame, Bk, for the kth link is simply the outboardhinge frame, Ok, for the kth hinge, i.e.,

Bk ≡ Ok (3.12)

Figure 3.3 illustrates a serial-chain that satisfies this assumption. For these systems,OkTk = I and (3.1) simplifies to

k+1Tk =

k+1T

O+k·O

+k TOk

(3.13)

Extensions for the more general situation when Bk �= Ok are discussed inSect. 3.3.3. The V(k) spatial velocity depends upon the spatial velocity of theinboard link frame, Ok+1, as well as on the motion of the hinge itself. The frameO

+k allows us to distinguish between these two contributions as follows:

V(k)3.12= V(Ok)

3.5= V+

(k)+ΔV(k) where V+(k)

�= V(O

+k ) (3.14)

V+(k) is the spatial velocity of the O+k frame and represents the contribution to

V(k) purely from the motion of the inboard (k+1)th link, while ΔV(k) representsthe contribution from the motion of the kth hinge. Since frames Bk+1 and O

+k are

both on the (k+ 1)th body, the spatial velocity of the O+k frame is related to the

spatial velocity of Bk+1 by the expression

V+(k)

1.33= φ∗

(Bk+1,O+k )V(k+ 1) (3.15)


O+k+1

O+k

O+k− 1

Ok+ 1

Ok

Ok− 1

p(k+1)p(k)

l(k+1,k) l(k,k−1)

kth

hinge


(k+1)th

hinge(k−1)th

hinge


CC

Fig. 3.3 Illustration of links and joints in a serial rigid body system with body frameBk = Ok

The φ(k+ 1,k) rigid body transformation matrix associated with the kth link hasthe form

φ(k+ 1,k)�=

(

I ˜l(Bk+1,Bk)

0 I

)

=

(

I ˜l(k+ 1,k)

0 I

)

(3.16)

wherel(k+ 1,k)

�= l(Bk+1,Bk) (3.17)

Since the location of the O+k frame coincides with the Ok frame, which in turn

coincides with Bk (per (3.12)), it follows that

φ(k+ 1,k)�= φ(Bk+1,Bk) = φ(Bk+1,Ok) = φ(Bk+1,O+

k ) (3.18)

If the kth hinge contains a prismatic component, then both l(k,k− 1) andφ(k,k− 1) will depend on the configuration of the (k− 1)th hinge.

Combining together (3.7), (3.14), (3.15) and (3.18) we obtain

V+(k)

3.15,3.18= φ∗

(k+ 1,k)V(k+ 1) (3.19a)

V(k)3.14,3.19a

= φ∗(k+ 1,k)V(k+ 1)+ΔV(k)

3.7= φ∗

(k+ 1,k)V(k+ 1)+H∗(k)β(k) (3.19b)

Equation (3.19b) shows how the spatial velocity of a link can be obtained fromthe spatial velocity of the inboard link and the relative spatial velocity across thehinge connecting the links. As illustrated in Fig. 3.4, the sequence V(1), · · · ,V(n)

3.3 Serial-Chain Kinematics 45

of spatial velocities for all the links can be obtained by means of the base-to-tiprecursive sweep described in Algorithm 3.2. The sequence begins at the base with

Algorithm 3.2 Recursive computation of link spatial velocities⎧⎪⎪⎪⎨⎪⎪⎪⎩

V(n+ 1) = 0

for k = n · · ·1V(k) = φ∗

(k+ 1,k)V(k+ 1)+H∗(k)β(k)

end loop

the initial condition V(n+ 1) = 0 (which is equivalent to saying that the inertialframe is stationary). The computations proceed in an outward direction from base-to-tip and ends with the computation of the V(1) spatial velocity of the tip-body.

V(1)

V(k)

V(k+1)

V(n)

Base to tiprecursion forbody spatialvelocities

Fig. 3.4 Recursive computation of link spatial velocities

3.3.3 Differential Kinematics with Bk �= Ok

The assumption in the previous section was that the Bk body frame coincided withthe Ok hinge frame for the kth body. In general, this assumption may not hold,and the body frame may not coincide with Ok. Such a system with Bk �= Ok is


O+k+ 1

O+k

O+k− 1

Ok+ 1

Ok

Ok− 1

Bk+ 1

Bk+ 2

Bk

Bk− 1

p(k+1)p(k)

l(k+1,k)

l(k,k−1)kth

hinge


(k+1)th

hinge(k−1)th

hinge


CC

Fig. 3.5 Illustration of links and joints in a serial rigid body system with body frameBk �= Ok

illustrated in Fig. 3.5. For this system, the more general form of (3.14) for the bodyspatial velocity is given by:

V(k)�= V(Bk) = φ∗

(Ok,Bk)V(Ok) (3.20)

The following exercise examines the modified velocity recursion relationships forthe bodies in the system.

Exercise 3.3 Velocity recursion with Bk �= Ok.Show that the new version of spatial velocity of the recursion in (3.19a) has thefollowing form:

V(k) = φ∗(k+ 1,k)V(k+ 1)+ΔB

V(k)

= φ∗(k+ 1,k)V(k+ 1)+H∗

B(k)β(k)(3.21)

where

ΔB

V(k)�= φ∗

(Ok,Bk)ΔV(k) and H∗B(k)

�= φ∗

(Ok,Bk)H∗(k) (3.22)

φ(k+ 1,k)�= φ(Bk+1,Bk)

= φ(Bk+1,Ok+1)φ(Ok+1,Ok)φ(Ok,Bk)(3.23)

ΔB

V(k) is the hinge relative spatial velocity referenced to the kth body frame Bk.

The expression for φ(k+ 1,k) in (3.23) generalizes (3.18).H∗

B(k) is the kth hinge’s joint map matrix referenced to the Bk body frame, and

is a generalization of H∗(k). Even when the kth hinge is a pure rotational hinge,

3.4 Spatial Operators 47

it follows from (3.22) that theH∗B(k) joint map matrix will have non-zero rotational

and translational components when Bk �= Ok.The recursive spatial velocity relationship in (3.21) represents a generalization of

(3.19b). The recursive Algorithm 3.2 continues to apply with H∗(k) replaced withH∗

B(k). We can thus, conclude that, apart from minor modifications when Bk �= Ok,

the relationships remain essentially the same. As a result, there is little generalitylost in continuing to assume that Bk = Ok.

Exercise 3.4 Velocity recursion with inertially fixed reference point.Exercise 2.14 introduced the notion of using an inertially fixed point I as the ve-locity reference point for a rigid body. In fact the same point I can be used as thevelocity reference point for all the links in the system. We use VI(k) to denote theinertially referenced spatial velocity for the kth link. It is defined as

VI(k)�= φ∗

(Ok,I)V(Ok) (3.24)

With

H∗I (k)

�= φ∗

(Ok,I)H∗(k) and ΔI

V(k)�= H∗

I (k)β(k) (3.25)

show that the inter-link spatial velocity recursive relationship is given by:

VI(k) = VI(k+ 1)+ΔI

V(k) = VI(k+ 1)+H∗I (k)β(k) (3.26)

For the inertially referenced case, we observe from (3.26) that the analog of thebody transformation operator φ(k+ 1,k), denoted φI(k+ 1,k), is simply the iden-tity matrix! However, this simplicity has been achieved at the cost of additionalcomplexity of the HI(k) hinge map matrix.

3.4 Spatial Operators

In the previous sections, we have developed component body-to-body velocity rela-tionships. In this section, we use these to develop corresponding relationships at thesystem level.

In the rest of this chapter, and without losing any generality, we make the follow-ing notationally convenient assumption regarding the hinges in the serial-chain:

We assume that β(.) = θ(.), and thus rp(·) = rv(·) for all the hinges.

In the general case when these assumptions do not hold, all that is required is that,wherever θ and θ appear in the dynamical and kinematical equations, they be re-placed by the appropriate β generalized velocities vector and the corresponding βacceleration vector, respectively.


The number of overall velocity degrees of freedom for the system is denoted N,and is defined as the sum of all the individual hinge velocity degrees of freedom, i.e.,

N�=

n∑k=1

rv(k) (3.27)

Now we introduce stacked vectors required to define system level relationships. Webegin by defining the stacked vectors V and θ as

V�= col

{V(k)

}n

k=1=

⎡

⎢

⎢

⎢

⎢

⎣

V(1)

V(2)

...

V(n)

⎤

⎥

⎥

⎥

⎥

⎦

∈R6n, and θ�= col

{θ(k)

}n

k=1=

⎡

⎢

⎢

⎢

⎢

⎣

θ(1)

θ(2)

...

θ(n)

⎤

⎥

⎥

⎥

⎥

⎦

∈RN

The V stacked vector consists of the component body-level V(k) spatial velocityvectors assembled into a single large vector. Correspondingly, the θ stacked vectorconsists of the component body-level θ(k) generalized coordinates assembled intoa single large vector. Similarly define the V+ and ΔV stacked vectors as:

V+ �= col

{V+

(k)}n

k=1∈R6n and ΔV

�= col

{ΔV(k)

}n

k=1∈R6n (3.28)

From these definitions, it follows that the component level (3.14) expression can beequivalently restated at the system-level as

V = V++ΔV (3.29)

Now define the strictly block lower-triangular spatial operator Eφ as:

Eφ�=

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0

φ(2,1) 0 . . . 0 0

0 φ(3,2) . . . 0 0

......

. . ....

...

0 0 . . . φ(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

∈R6n×6n (3.30)

It is easy to verify that the component level (3.19a) relationship, V+(k) =

φ∗(k+ 1,k)V(k+ 1), can be equivalently re-expressed using Eφ and the stackedvectors as:

V+= E∗

φV (3.31)

3.4 Spatial Operators 49

Continuing on, define the block-diagonalH spatial operator as

H�= diag

{H(k)

}n

k=1=

⎛

⎜

⎜

⎜

⎜

⎝

H(1) 0 . . . 00 H(2) . . . 0...

.... . .

...

0 0 . . . H(n)

⎞

⎟

⎟

⎟

⎟

⎠

∈RN×6n (3.32)

Using the H operator, the component-level relationship in (3.7) can be re-expressedat the system level as:

ΔV =H∗ θ (3.33)

Combining (3.29), (3.31) and (3.33), we obtain the relationship

V = E∗φV+H∗θ (3.34)

Equation (3.34) is an equivalent compact version of the link-level velocity relation-ship in (3.19b) for all the links in the system. Equation (3.34) is an implicit relation-ship because V appears on both sides of the equality. Rearranging terms, (3.34) canbe restated as

(I−E∗φ)V =H∗ θ (3.35)

3.4.1 The φ Spatial Operator

Being strictly lower-triangular, Eφ is nilpotent,4 i.e., its power vanishes for somefinite exponent. In fact, En

φ = 0. With this property, we can make use of LemmaA.1 on page 400 which discusses inverses related to nilpotent matrices and providesan explicit expression for the inverse of (I−Eφ). From Lemma A.1, the inverse of(I−Eφ), denoted φ ∈R6n×6n, exists and has the form

φ�= (I−Eφ)

−1= I+Eφ +E2

φ + · · · +En−1φ (3.36)

Exercise 3.5 Internal structure of the φ operator.Show that the φ spatial operator has the following block lower-triangular form

φ=

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0

φ(2,1) I . . . 0...

.... . .

...

φ(n,1) φ(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

(3.37)

4 A matrix X is said to be nilpotent if there exists an integer, n, such that Xn = 0.


Verify that for i > j, the φ(i, j) ∈R6×6 elements of φ are also rigid body transfor-mation operators and are given by:

φ(i, j) = φ(i, i− 1) · · · φ(j+ 1, j) =

(

I ˜l(i, j)

0 I

)

(3.38)

l(i, j) denotes the vector from the ith link frame Bi, to the jth link frame Bj. Equa-tion (3.38) is a carry-over of the (1.32) semi-group property of the φ(i, j) elements.

The φ spatial operator is generally configuration dependent since its φ(k, j)elements are configuration dependent.

3.4.2 Velocity Operator Expression

Now that we have an expression for the inverse of (I−Eφ), we use it to obtain thefollowing explicit expression for the V spatial velocity stacked vector:

V3.35= (I−E∗

φ)−1H∗ θ 3.36

= φ∗H∗ θ (3.39)

Before proceeding further, an important point to emphasize is that the system-levelrelationship between the stacked vectors θ and V, as defined by the spatial op-erator expression in (3.39), is derived from and mathematically equivalent to thecomponent-level relationship in (3.19b).

However, the computational story is different. On the face of it, the expressionV = φ∗H∗ θ in (3.39) suggests that the evaluation of V from θ requires at least twomatrix vector products, with the first being ΔV =H∗ θ and the second being φ∗ΔV.While the first product involving the block-diagonal H∗ matrix can be carried outat O(N) cost, the latter product involves the populated, though upper-triangular,φ∗ matrix and is of O(N2) computational cost. Hence, the overall cost of com-puting V through this process is of O(N2) computational complexity. In contrast,Algorithm 3.2 involves a base-to-tip recursion for computing the elements of V, aprocess that scales linearly with the number of bodies and is of just O(N) compu-tational complexity. Thus, we see here the existence of a direct recursive computa-tional algorithm of simpler complexity than would be suggested by the brute forcematrix/vector products implied by the V = φ∗H∗ θ operator expression.

This is just the first of many examples we will encounter, where the special struc-ture of the spatial operators allows us to implement computational algorithms thatare far less expensive than those implied by the operator expressions. Indeed, inSect. 3.5 we will start to examine patterns that allow us to directly generate such sim-pler recursive computational algorithms by direct examination of spatial operatorexpressions. We will see that while spatial operators provide the means to conciselyrepresent complex multibody relationships, and the tools to carry out mathemati-cal analysis of multibody quantities, they also provide a straightforward avenue for

3.5 Recursions Associated with the φ Operator 51

converting operator relationships into low-order computational algorithms whenthe expressions need to be evaluated.

3.4.3 The φ Spatial Operator

Define the spatial operator φ ∈R6n×6n as

φ�= φ− I (3.40)

Observe that φ is strictly lower-triangular. The following exercise establishes basicproperties of φ.

Exercise 3.6 The φ spatial operator.

1. Show thatφ= Eφφ= φEφ (3.41)

2. Show that the stacked vector of spatial velocities V+ satisfies the relationship

V+= φ∗H∗ θ (3.42)

3.5 Recursions Associated with the φ Operator

We have seen how the apparent O(N2) procedure for computing the body spatialvelocities, V, can in fact be replaced by anO(N) recursive algorithm. In this sectionwe identify principles and structural properties of spatial operators that make thispossible more generally. We begin with the following lemma which shows how anexpression such as y = φx can be evaluated using a tip-to-base O(N) recursivealgorithm involving only the φ(k+ 1,k) elements and x.

Lemma 3.1 Tip-to-base recursion to evaluate φx.Given a stacked vector x ∈ R6n, the y= φx operator expression can be evaluatedusing the followingO(N) tip-to-base recursive algorithm:

y= φx ⇔

⎧⎪⎪⎪⎨⎪⎪⎪⎩

y(0) = 0

for k = 1 · · ·ny(k) = φ(k,k− 1)y(k− 1)+x(k)

end loop

(3.43)


The structure of this tip-to-base computational algorithm is illustrated on the leftside of Fig. 3.6.Proof: Using the expression for φ in (3.37), it follows that the expression for thekth element of y= φx is given by:

y(k)3.37=

k∑j=1

φ(k, j)x(j) (3.44)

Thus,

y(k)3.38= φ(k,k− 1)

k−1∑j=1

φ(k− 1, j)x(j)+x(k)

3.44= φ(k,k− 1)y(k− 1)+x(k)

This establishes the result.This lemma illustrates how recursive dynamical algorithms can be derived natu-

rally by exploiting the special semi-group properties of the elements of theφ spatialoperator. Thus given a stacked vector x, evaluating the matrix–vector product φxdoes not require an O(N2) operator-vector product computation, and not even theexplicit computation of the elements of φ!

Similarly, the product x = φ∗y is equivalent to a base-to-tip O(N) recursivecomputational algorithm as described in the following lemma.

y = φx y = φ∗x y( )y( )

y( )y( )

y( ) =

φ( , − )y( − )

+x( )

y( ) =

φ∗( + , )y( + )

+x( )

Tip to baserecursio

Base to tiprecursio

1111

1 11 1

k

kkk

k

k

kkk

kk k

nn

n nn n

Fig. 3.6 Canonical tip-to-base and base-to-tip recursion to evaluate φx and φ∗x,respectively

Lemma 3.2 Base-to-tip recursion to evaluate φ∗x.Given a stacked vector x∈R6n, the y=φ∗x operator relationship can be evaluated

3.6 The Jacobian Operator 53

using the followingO(N) base-to-tip recursive algorithm:

y= φ∗x ⇔

⎧⎪⎪⎪⎨⎪⎪⎪⎩

y(n+ 1) = 0

for k = n · · ·1y(k) = φ∗

(k+ 1,k)y(k+ 1)+x(k)

end loop

(3.45)

The structure of this base-to-tip computational algorithm is illustrated on the rightside of Fig. 3.6.Proof: Using the expression for φ in (3.37), we have

y(k)3.37=

k∑j=n

φ∗(j,k)x(j) (3.46)

Thus,

y(k)3.38= φ∗

(k+ 1,k)k+1∑j=n

φ∗(j,k+ 1)x(j)+x(k)

3.46= φ∗

(k+ 1,k)y(k+ 1)+x(k)

This establishes the result.The equivalence described in (3.45) is precisely the correspondence between the

concise operator-based V = φ∗H∗ θ velocity relationship in (3.39) and the equiva-lent recursive algorithm for the link spatial velocities in Algorithm 3.2 on page 45,with x=H∗ θ and y= V.

Exercise 3.7 Recursive evaluation of φx and φ∗x.Develop O(N) recursive algorithms similar to those in (3.43) and (3.45) for evalu-ating φx and φ∗x.

Section 9.2 on page 162 extends the φ operator and related recursions to moregeneral multibody systems such as tree-topology systems, where the link numberingcan be arbitrary and the φ and φ∗ operators are not necessarily triangular.

3.6 The Jacobian Operator

Robotic multibody systems typically have distinguished frames of interest (alsoreferred to as nodes) that need to monitored and/or controlled. The end-effectorframe is an example of such a node for robotic arms and is generally located on


the outer-most link. As illustrated in Fig. 3.7, these nodes may not coincide withthe body frames or the hinge frames. Also, a body can have none to multiple suchnodes. In this section, we explore the Jacobian operators that define the differentialrelationship between the generalized velocities of the system and the spatial veloci-ties of such distinguished nodes.

nodes

Fig. 3.7 Illustration of nodes on a serial-chain system

Since individual bodies can have multiple nodes, we introduce the notation Oik

to denote the ith node on the kth link. The number of nodes on the kth body isdenoted nnd(k). A single end-effector frame located on link 1 would be denotedO0

1. The spatial velocity of the Oik node frame is denoted V

(

Oik

)

, and is related tothe V(k) spatial velocity of the kth link by

V(

Oik

)

1.33= φ∗

(k,Oik)V(k) (3.47)

where φ(k,Oik) is the rigid body transformation matrix from the kth link frame,

Bk, to the rigidly attached Oik node frame. Let nnd denote the total number of such

nodes across all the links in the system, i.e.,

nnd�=

n∑k=1

nnd(k) (3.48)

Now define the 6nnd dimensional stacked vector, Vnd as consisting of the spatialvelocities of all the nodes on all the bodies in the system, i.e.,

3.6 The Jacobian Operator 55

Vnd(k)�= col

{V(

Oik

)}nnd(k)

i=1∈R6nnd(k)

Vnd�= col

{Vnd(k)

}n

i=1∈R6nnd

(3.49)

The component-level relationship in (3.47) implies the following system-level rela-tionship between the V and Vnd stacked vectors

Vnd3.47= B∗V (3.50)

where the 6n× 6nnd dimensional pick-off spatial operator B is a block-matrixwhose row indices are the bodies, and the column indices are the nodes in the sys-tem. B contains one 6n× 6 dimensional block-column of 6× 6 matrices for eachOi

k node. The column for the Oik node has φ(k,Oi

k) as the only non-zero elementat the kth parent link slot. While each column of B has only one non-zero 6× 6matrix element, its kth row can have multiple 6×6 non-zero matrix elements, onefor each node on the kth link. B is refereed to as a pick off spatial operator, since itpicks off and maps the link spatial velocities into the spatial velocities of the nodes,as per (3.50). When the only node is a single end-effector node on the outermostlink, B has the form

B�=

⎡

⎢

⎢

⎢

⎢

⎣

φ(1,O01)

0...

0

⎤

⎥

⎥

⎥

⎥

⎦

∈R6n×6 (3.51)

Using the expression for V in (3.39), (3.50) can be re-expressed as

Vnd3.50,3.39

= B∗φ∗H∗ θ (3.52)

In other words,

Vnd = Jθ where J�= B∗φ∗H∗ ∈R6nnd×N (3.53)

J is referred to as the Jacobian operator for the serial-chain. It maps the θ general-ized velocities into the Vnd spatial velocities of the nodes for the serial-chain [41].The Jacobian operator J in (3.53) is the product of three operators B∗, φ∗ and H∗.The action of the Jacobian operator on the joint angle rates θ is as follows:

1. H∗ θ results in the ΔV relative spatial velocities across the hinges.2. φ∗ then propagates these relative velocities from the base-to-tip (Algorithm 3.2)

to obtain the link spatial velocities.3. B∗ then picks out the V(k) link spatial-velocities from V and propagates them to

the nodes to obtain their V(

Oik

)

spatial velocities.

The key point to note is that J has a operator factorization which has immediatephysical interpretation and obvious recursive algorithm equivalents. The operatorfactorization of the Jacobian matrix will play an important part in studying thekinematics and dynamics of systems in later chapters.

Chapter 4The Mass Matrix

While the mass and rotational inertia of a body are the elemental quantities forstudying single rigid body dynamics, the mass matrix plays the corresponding rolefor multibody systems. One crucial difference is that the mass matrix varies withthe configuration of the system. In this chapter, we derive expressions for the massmatrix of a serial-chain multibody system, study its properties, and develop relatedcomputational algorithms.

4.1 Mass Matrix of a Serial-Chain System

4.1.1 Kinetic Energy of the Serial-Chain

For a serial-chain system, we use (2.7) to define theM(k) spatial inertia of the kthlink about its body frame Bk as follows:

M(k)�=

(

J (k) m(k) p(k)

−m(k) p(k) m(k)I

)

(4.1)

J (k) is the inertia tensor for the kth link about Bk, p(k) is the vector from Bk tothe center of mass of the kth link, and m(k) is the mass of the kth link.

The total kinetic energy of the system, Ke, is the sum of the kinetic energies ofeach of the individual links:

Ke2.5=

12

n∑k=1

V∗(k)M(k)V(k) (4.2)


58 4 The Mass Matrix

Equation (4.2) can be re-expressed as

Ke =12[V∗

(1), · · · ,V∗(n)]

⎛

⎜

⎜

⎜

⎜

⎝

M(1) 0 · · · 00 M(2) · · · 0...

.... . .

...

0 0 · · · M(n)

⎞

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎝

V(1)

V(2)

...

V(n)

⎞

⎟

⎟

⎟

⎟

⎠

=12V∗MV (4.3)

where M is the block-diagonal, symmetric and positive semi-definite spatial iner-tia spatial operator defined as

M�= diag

{M(k)

}n

k=1∈R6n×6n (4.4)

Equation (4.3) expresses the total system kinetic energy in terms of the spatial ve-locity vector V. Substituting V =φ∗H∗ θ from (3.52) into (4.3) allows us to expressthe kinetic energy of the serial-chain in terms of the generalized velocities θ:

Ke3.39,4.3

=12θ∗HφMφ∗H∗ θ =

12θ∗M(θ)θ (4.5)

whereM(θ)

�= HφMφ∗H∗ ∈RN×N (4.6)

Equation (4.5) has the familiar quadratic form for kinetic energy. M in (4.6) is re-ferred to as the mass matrix of the multibody system. Since M is symmetric andpositive semi-definite, so is the M mass matrix. However, since φ is configuration-dependent, the mass matrix is also configuration-dependent. The mass matrix is anatural generalization of the notion of spatial inertia of single rigid bodies for artic-ulated multibody systems and plays a central role in their dynamics.

Equation (4.6) can also be viewed as a spatial operator factorization of the massmatrix. This factored form of the mass matrix is referred to as the Newton–EulerOperator Factorization of the mass matrix. We will further explore the rich in-ternal structure of the mass matrix in Chap. 7, and develop an alternative analyticalInnovations Operator Factorization for it, as well as an expression for its inverse.

We now turn to the problem of computing the configuration dependent massmatrix. Possible methods include:

1. The Lagrangian approach which consists of forming a configuration dependentexpression for the kinetic energy in (4.2), and taking its second order partialderivative with respect to the generalized velocities to obtain the mass matrixelements. This approach can become very complex for even moderately sizedsystems.

2. An approach based on (4.6). Here, the component operators φ, M, and H arecomputed explicitly, and the mass matrix is computed by evaluating the prod-

4.1 Mass Matrix of a Serial-Chain System 59

ucts in (4.6). This approach is also computationally quite expensive with O(N3)

computational complexity in the number of degrees of freedom.3. The most efficient approach, with only O(N2) computational complexity, is

based on composite body inertias. It is discussed in the next section.

4.1.2 Composite Rigid Body Inertias

The composite body inertia associated with a hinge is defined as the effective spa-tial inertia of the composite body formed by all of the bodies outboard of the hinge.This is illustrated in Fig. 4.1. Thus, the composite body inertia for the kth link,

R(1)

R(2)

R(3)

R(n)

1

2

3

n

Fig. 4.1 Physical interpretation of composite rigid body inertias

denoted R(k), is the effective spatial inertia about Bk of the k · · · 1 links, assumingthat they form a rigid body obtained by freezing hinges (k− 1) · · ·1. The value ofR(k) is not a constant and depends on the configuration of the 1 through (k− 1)

hinges.Now let us examine how we might go about computing the composite rigid body

inertias for all the links. Clearly, for the outer-most link, R(1) =M(1), the spatialinertia of link 1. R(2) is the spatial inertia at B2 of links 1 and 2 combined toform a composite rigid body by “freezing” hinge 1 and ignoring the inboard links.Similarly, the composite rigid body inertia, R(k), at the kth link can be obtained byusing the parallel axis theorem for spatial inertias to combine together the compositerigid body inertia R(k− 1) at link (k− 1), with the spatial inertia,M(k), of the kthlink, i.e.,

R(k) = φ(k,k− 1)R(k− 1)φ∗(k,k− 1)+M(k) (4.7)


We can thus, compute all the composite inertias starting from the tip body andproceeding towards the base. This O(N) tip-to-base recursive computationalprocess is described in Algorithm 4.1 and illustrated in Fig. 4.2.

Algorithm 4.1 Recursive computation of Composite Body Inertias⎧⎪⎪⎪⎨⎪⎪⎪⎩

R(0) = 0

for k = 1 · · ·nR(k) = φ(k,k− 1)R(k− 1)φ∗

(k,k− 1)+M(k)

end loop

(4.8)

This recursive procedure is also known as a Lyapunov recursion1 since it is closelyassociated with the Lyapunov equation for composite body inertias, which we willencounter in the next section. Algorithm 9.2 on page 169 describes the generaliza-tion of this composite body inertia algorithm for tree-topology systems.

R(1) = M(1)

R(n)

R(k) = φ(k,k−1)R(k−1)φ∗(k,k−1)+M(k)

1

k

n

Tip to baserecursion

Fig. 4.2 Recursive computation of composite rigid body inertias

Since R(k) is the composition of spatial inertias, it is itself a spatial inertia. Thus,it can be parameterized by just ten parameters: one for the mass, three for the masscenter location, and six for the rotational inertia. This reduces the number of com-putations required to evaluate the composite body inertias.

1 Section 6.4 on page 111 discusses the roots of this terminology which arise from the mathematicalparallels with estimation theory.


Remark 4.1 R(k) does not depend upon inboard bodies.The composite body inertia R(k) depends only on the outboard hinge coordinatesθ(i) for i < k and is independent of the hinge coordinates of the inboard links.

4.1.3 Decomposition of φMφ∗

Define R�= diag

{R(k)

}n

k=1∈R6n×6n to be the block-diagonal, symmetric, pos-

itive semi-definite operator with the kth block-diagonal entry being R(k). The fol-lowing lemma shows that the φMφ∗ operator product can be decomposed intodiagonal and triangular factors using the R operator.

Lemma 4.1 Lyapunov decomposition using composite body inertias.R satisfies the operator equation

M = R−EφRE∗φ (4.9)

In addition, φMφ∗ can be decomposed into the following disjoint sum:

φMφ∗= R+ φR+Rφ∗ (4.10)

In this disjoint partitioning of φMφ∗, the first term on the right-hand side is block-diagonal, the second is strictly lower-triangular, and the last term is strictly upper-triangular as illustrated in Fig. 4.3.Proof: It is a straightforward exercise to verify that EφRE∗

φ is block-diagonalwith the kth k diagonal entry being φ(k,k−1)R(k−1)φ∗(k,k−1). Thus (4.9) ismerely a restatement in stacked notation of the recursions in (4.8).

Multiplying (4.9) from the left and right by φ and φ∗, respectively then leads to

φMφ∗ 4.9= φRφ∗

−φEφRE∗φφ

∗ 3.41= φRφ∗

− φRφ∗

3.40= (φ+ I)R(φ+ I)− φRφ∗ 3.40

= R+ φR+Rφ∗

This establishes (4.10).Since φ is strictly lower-triangular and R is block-diagonal, it follows that φR is

strictly lower-triangular.Equation (4.9) is identical in form to the discrete Lyapunov equations that arise

in the theory of linear discrete-time systems [5]. R is block-diagonal, φR is strictlylower-triangular, and Rφ∗ strictly upper-triangular. The three terms, R, φR, andRφ∗, on the right in (4.10) can be computed from just the knowledge of R and φ.Due to its symmetry, computing φMφ∗, requires computing just the R and φR

terms. Furthermore, with the knowledge of R, the φR product can be computedrecursively. This is so because the block entries in the jth column of φR have theform:


φR

R

Rφ∗

φMφ∗ =

strictly uppertriangular

strictly lowertriangular

blockdiagonal

Fig. 4.3 The decomposition of φMφ∗ into diagonal and triangular components

[

φR]

(k, j) =

{0 for 1 � k� jφ(k, j)R(j) for j < k� n

(4.11)

Since φ(k+ 1, j) = φ(k+ 1,k)φ(k, j), this implies that

[

φR]

(k+ 1, j)4.11= φ(k+ 1,k) · [φR

]

(k, j) (4.12)

This forms the basis for a recursion sequence, starting from the R(j) diagonal el-ement, to compute the jth column elements of φR, ending up with the (n, j)thelement of φR. We will exploit this structure in the following section when devel-oping efficient procedures for computing the mass matrix M.

Exercise 4.1 System center of mass.This exercise explores the instantaneous properties, such as the spatial inertia, thecenter of mass location, and angular momentum, for the overall system.

1. Show that the overall spatial inertia of the system referenced to the Bn base-bodyframe,MS, is the composite body inertia R(n), for the base-body, i.e.,

MS = R(n) = E∗RE (4.13)

where E is the base pick-off operator (dual of the B tip pick-off operator)defined as

E�= [06, · · · 06, I6] ∈R6×6n (4.14)


The first moment vector, l(n,CS) of R(n) specifies the instantaneous location ofthe center of mass of the system, CS, with respect to the reference frame Bn ofthe base-body.

2. Show that the instantaneous spatial momentum of the system, hS, referencedabout the Bn base-body frame, is given by the expression

hS = EφRH∗ θ (4.15)

For a free-flying system, i.e., one whose nth hinge is a full 6 degree of freedomhinge, show that (4.15) can be re-expressed as:

hS = R(n)V(n)+

n−1∑k=1

φ(n,k)R(k)H∗(k)θ(k) (4.16)

3. Show that the instantaneous spatial velocity VC, of the center of mass of thesystem, referenced to the Bn frame, is given by the expression:

VC =M−1S hS = R−1

(n)

n∑k=1

φ(n,k)R(k)H∗(k)θ(k) (4.17)

4. For a free-flying system, show that (4.17) simplifies to:

VC = V(n)+R−1(n)

n−1∑k=1

φ(n,k)R(k)H∗(k)θ(k) (4.18)

Hence, show that an additional spatial velocity of −VC at the base-body nullifiesthe spatial momentum of the system, i.e., it makes hS = 0.

Since VC represents the spatial velocity of the center of mass of the system ref-erenced to Bn, the spatial velocity of the system center of mass with respect to theinertial frame is φ∗(Bn,CS)VC.

The next section uses the decomposition in (4.10) to develop a recursiveO(N2)

algorithm for evaluating the configuration dependent mass matrix.

4.1.4 O(N2) Algorithm for Computing the Mass Matrix

Lemma 4.2 Decomposition of the mass matrix.The mass matrix can be decomposed into disjoint diagonal and strictly triangularterms as follows:

M =HRH∗+HφRH∗

+HRφ∗H∗ (4.19)


Proof: The result follows by pre- and post-multiplying (4.10) with H and H∗,respectively, and recalling from (4.6) that M = HφMφ∗H∗. Since H is block-diagonal, the disjoint property of the decomposition in (4.10) continues to hold for(4.19).

From this decomposition and (4.11), it follows that the elements of M are givenby the following expressions:

M(i, j) =

⎧⎪⎨⎪⎩H(i)R(i)H∗(i) for i= j

H(i)φ(i, j)R(j)H∗(j) for i > j

M∗(j, i) for i < j

(4.20)

Thus, the M(i, j) element of the mass matrix depends only upon H(i), φ(i, j) andR(j). This means that M(i, j) depends only upon θ(k), where k is in the range(1 · · ·max(i, j). That is, the element depends only upon the generalized coordinatesof the hinges outboard of the ith and jth hinges, and is independent of the general-ized coordinates of the inboard hinges!

Combining (4.8) and (4.19), we obtain the O(N2) Algorithm 4.2 for computingthe mass matrix M. This algorithm includes Algorithm 4.1 for computing the com-posite body inertias, and makes use of the recursive expression in (4.12) and (4.20)for the off-diagonal terms.

Algorithm 4.2 Recursive computation of the mass matrix⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

R(0) = 0

for k = 1 · · ·nR(k) = φ(k,k− 1)R(k− 1)φ∗

(k,k− 1)+M(k)⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

X(k) = R(k)H∗(k), M(k,k) =H(k)X(k)

for j = (k+ 1) · · ·nX(j) = φ(j, j− 1)X(j− 1)

M(j,k) = M∗(k, j) =H(j)X(j)

end loop

end loop

This algorithm evaluates the lower-triangular mass matrix elements represented bythe elements of the HRH∗ and HφRH∗ terms in (4.19). Since the mass matrix issymmetric, this is sufficient to evaluate all of the elements of the mass matrix.

The outer loop of the algorithm computes the R(k) matrices recursively in a tip-to-base recursion. As illustrated in Fig. 4.4, as each R(k) is computed, the inner-looprecursively computes the lower-diagonal elements of the kth column of the massmatrix.


R(1)

R(k)

R(n)

upper triangular ele-ments by symmetryfrom lower triangularelements

kth colM(.,k)elements

1st colM(., 1)elements

Fig. 4.4 Structure of the recursive algorithm for computing the mass matrix usingcomposite body inertias

This composite body inertia algorithm for computing the mass matrix is anotherexample of the development of computationally efficient algorithms using spatialoperators. This algorithm corresponds to Option 3 in the mass matrix computationoptions described on page 59. It is the most efficient option.

Exercise 4.2 Trace of the mass matrix.Derive the following expression for the trace of the mass matrix:

Trace {M(θ)} =

n∑i=1

Trace {H(k)R(k)H∗(k)} (4.21)

For a 1 degree of freedom hinge, Trace {H(k)R(k)H∗(k)} =H(k)R(k)H∗(k). Thiscan be used to further simplify (4.21).

4.1.5 Relationship to the Composite Rigid Body Method

We now show that Algorithm 4.2 can be interpreted as a version of what is referredto as the composite body method for computing M developed in Walker and Orin[179]. We show this correspondence by expressing the above recursions in termsof the changes undergone by the mass, the mass center location, and the rotationalinertia of the composite body composed of the links outboard of hinge k−1 as bodyk is added to it.


Since R(k) is a spatial inertia at Bk, it can be expressed as

R(k) =

(

J(k) ˜l(k,Ck)ρ(k)

−ρ(k)˜l(k,Ck) ρ(k)I

)

(4.22)

in which ρ(k), Ck, and J(k) are the mass, the location of the center of mass, and therotational inertia of the composite body formed by links 1, . . . ,k. l(k,Ck) denotesthe vector from Ok to Ck. Substitution of (4.22) for R(k− 1) in Algorithm 4.2, anddefining

l(k,Ck−1)�= l(k,k− 1)+ l(k− 1,Ck−1)

leads to

ρ(k) = ρ(k−1)+m(k)

l(k,Ck)ρ(k) = l(k,Ck−1)ρ(k−1)+m(k)p(k)

J(k) = J(k−1)+ρ(k−1)[l∗(k,Ck−1)l(k,Ck−1)I−l(k,Ck−1)l∗(k,Ck−1)]

−ρ(k−1)[l∗(k−1,Ck−1)l(k−1,Ck−1)I

−l(k−1,Ck−1)l∗(k−1,Ck−1)]+J (k)

A.1= J(k−1)−ρ(k−1)˜l(k,Ck−1)

˜l(k,Ck−1)

+ρ(k−1)˜l(k−1,Ck−1)˜l(k−1,Ck−1)+J (k)

(4.23)

Equation (4.23) computes the new mass ρ(k) for the composite body consisting oflinks 1 through k, by adding the mass of body k to the old mass ρ(k− 1). Similarly,it computes the new mass center location vector l(k,Ck). Comparing with (2.13),we see that the last equation computes the inertia tensor J(k) for the composite body.Equation (4.23) describes the terms that are used in the composite rigid body inertiamethod described in Walker and Orin [179] for the mass matrix. Algorithm 4.2 canbe viewed as the composite rigid body inertia method expressed more compactlyusing spatial quantities.

4.2 Lagrangian Approach to the Equations of Motion

The approach for formulating the system-level equations of motion described in thissection is referred to as the Lagrangian approach. In this approach, the equationsof motion of a system can be derived from just the system Lagrangian, which for ourmultibody systems is typically just the kinetic energy. This technique is very generaland is independent of the multibody system topology. The key quantities character-izing the dynamics of the systems are the mass matrix M(θ) and the Coriolis forcesvector C(θ, θ). The Lagrangian approach requires that:

• The generalized coordinates vector θ be of minimal dimension• The generalized velocity vector β be θ

4.2 Lagrangian Approach to the Equations of Motion 67

The Lagrangian equations of motion have the following form [11, 60, 111]:

ddt∂L

∂θ−∂L

∂θ= T (4.24)

T denotes the vector of generalized forces and L the Lagrangian for the system. Thenotation for the derivative and gradient with respect to vectors in (4.24) is describedin more detail in Sect. A.6.2 on page 401 in the appendix.

In the absence of a potential field, the Lagrangian L of a system is simply itskinetic energy. As is the case for multibody systems, we assume that the kineticenergy has the following quadratic form:

L = Ke4.5=

12θ∗M(θ)θ (4.25)

where the configuration dependent, symmetric matrix M(θ) ∈RN×N is referred toas the mass matrix of the system. Beyond borrowing the mass matrix terminologyand the M notation from that for serial-chain systems, we have made no specificassumptions that limit the discussion to serial-chain systems. Using (4.25) in (4.24)leads to equations of motion of the form

M(θ)θ+C(θ, θ) = T (4.26)

where

C(θ, θ)�= M(θ)θ−

12

∂[

θ∗M(θ)θ

]

∂θ∈RN (4.27)

C(θ, θ) is referred to as the Coriolis and gyroscopic forces vector. It depends non-linearly upon both the configuration and velocity coordinates for the system.

4.2.1 Properties of M and C

In this section we look at the properties of the mass matrix, M, and the Coriolisforces vector, C, associated with the Lagrangian form of the equations of motion in(4.26) and (4.27). First, define the generalized momentum of the system, p, as

p�= Mθ (4.28)

Exercise 4.3 The MD(θ, θ) matrix.Define the matrix MD(θ, θ) ∈RN×N [180] as the gradient of the generalized mo-mentum with respect to the generalized coordinates, i.e.,

MD(θ, θ)�= ∇θ(p) = ∇θ(Mθ) (4.29)


1. Show that

2∂Ke

∂θ

4.25=

∂(θ∗Mθ)

∂θ=M∗

D θ (4.30)

2. Show that for arbitrary vectors θ and θ:

M(θ)θ =MD(θ, θ)θ (4.31)

The MD(θ, θ) matrix plays an important role in energy-based controller designfor robot manipulators [8, 105, 180, 182]. Remark 18.1 on page 365 derives an ex-plicit expression for the elements of MD. It follows from (4.27), (4.30) and (4.31)that the C Coriolis vector can be expressed as:

C(θ, θ) =

[

MD −12M∗

D

]

θ (4.32)

Exercise 4.4 Mθ− 2C is a non-working force.

1. Show thatMθ− 2C = (M∗

D −MD)θ (4.33)

Observe that (M∗D −MD) is a skew-symmetric matrix.

2. Show that the generalized forces vector defined by Mθ− 2C does no work.3. Show that the C Coriolis forces vector does do work, i.e., θ

∗C(θ, θ) �= 0.

Exercise 4.5 Rate of change of the kinetic energy.For (4.25), show that the rate of change of the kinetic energy is given by

dKe

dt= θ

∗T (4.34)

An immediate consequence of this result is that, as expected, the system kineticenergy is conserved when the T generalized forces vector is zero.

Exercise 4.6 Christoffel symbols of the first kind.Christoffel symbols of the first kind are defined as2

Ci(j,k)�=

12

[

∂M(i, j)∂θ(k)

+∂M(i,k)∂θ(j)

−∂M(j,k)∂θ(i)

]

for i, j,k= 1 · · ·n(4.35)

2 The notation, [ij,k], is also often used for Ck(i,j) in the literature for Christoffel symbols ofthe first kind.


1. Show that the Christoffel symbols satisfy the following properties:

Ci(j,k) = Ci(k, j)

∂M(i, i)∂θk

= 2Ci(i,k)(4.36)

If, in addition, the mass matrix M(θ) is symmetric, then show that

∂M(k, j)∂θi

= Cj(i,k)+Ck(j, i) (4.37)

2. For multibody systems, the mass matrix is always symmetric. Also, for trees, theM(j,k) element only depends on the configuration coordinates for bodies that areoutboard of the jth or the kth body. For such systems, show that the Christoffelsymbols satisfy the following additional properties:

Ci(j,k) = −Ck(j, i) ∀ j� i,kCi(j, i) = Ci(i, j) = 0 ∀ j� i (4.38)

Also, show that Ci(j,k) is a function of θ(1) · · ·θ(m− 1) alone, where m�=

max(i, j,k).3. Christoffel symbols provide an alternate way for obtaining the Coriolis forces

vector C(θ, θ). Let Ci be the symmetric matrix whose (j,k)th element is given bythe Christoffel symbol Ci(j,k). Show that the ith element, C(i), of the Coriolisforces vector in (4.27) is given by the expression:

C(i) = θ∗Ci θ (4.39)

Thus it follows from (4.39) that

C(θ, θ)4.39=

⎡

⎢

⎢

⎣

θ∗C1 θ

...

θ∗Cn θ

⎤

⎥

⎥

⎦

(4.40)

4. For a symmetric mass matrix M show that

MD(i, j) =

N∑k=1

∂M(i,k)∂θ(j)

θ(k) =

N∑k=1

[

Ck(i, j)+Ci(k, j)]

θ(k) (4.41)


4.2.2 Hamiltonian Form of the Equations of Motion

The following exercise derives the Hamiltonian form of the equations of motion.This is an alternative to the Lagrangian form in (4.24). One advantage of the Hamil-tonian form is that the equations of motion are in the form of first order ordinarydifferential equations instead of the second order ones in Lagrangian formulations.

Exercise 4.7 Hamiltonian form of the equations of motion.Observe that the kinetic energy in (4.5) can be expressed in terms of the generalizedmomentum p as:

Ke =12p∗M−1

p (4.42)

With the system Hamiltonian defined by H�= Ke, derive the following Hamiltonian

form of the equations of motion

θ =∂H

∂pand p = −

∂H

∂θ+T (4.43)

from the Lagrangian form in (4.24).

4.2.3 Transformation of Lagrangian Coordinates

In this section, we examine the impact of transforming the system generalized coor-dinates on the Lagrangian equations of motion. The Lagrangian equations of motionin (4.24) require that the generalized velocity vectorβ be chosen as θ. However, thiscondition did not hold for our earlier choice of angular velocities for generalized ve-locities for a rigid body. The following exercise generalizes (4.24) to the case whereβ �= θ, and where β can in fact consist of quasi-velocities.

Exercise 4.8 Lagrangian equations of motion using quasi-velocities.

Let β�= A(θ)θ denote a new generalized velocity vector for the system with A(θ)

being a differentiable, non-singular, coordinate dependent transformation matrix.The elements of β are quasi-velocities when they are non-integrable. The newLagrangian is defined as L(θ,β) = L(θ, θ). Show that the Lagrangian equationsof motion in the (θ,β) coordinates have the following form:

ddt∂L

∂β−A−∗∂L

∂θ+A−∗γ∗

∂L

∂β=A−∗T (4.44)

with the matrix γ(θ, θ) defined as


γ�= A−∇θ(Aθ) = A−∇θβ(θ) (4.45)

The γ(i, j) matrix elements are given by

γ(i, j)�= A(i, j)−

∂[Aθ](i)

∂θ(j)=

∑k

(

∂A(i, j)∂θ(k)

−∂A(i,k)∂θ(j)

)

θ(k) (4.46)

The Lagrangian formulation of the equations of motion using quasi-velocities arealso discussed in Duindam and Stramigioli [45]. In the language of tensor analysis,γ, as defined in (4.45), is a measure of the non-closedness, i.e., the non-integrability,of the differential form defined by A.

Remark 4.2 The angular velocity as quasi-velocities.While the equations of motion in (4.44) based on quasi-velocities appear more com-plex than those in (4.24), the rigid body rotational equations of motion are a goodexample where the converse is true.

Focusing on just the rotational dynamics of a rigid body, we choose its angularvelocity,ω, as the generalized velocities for the body. For this case, the LagrangianL = 1

2ω∗Jω is independent of the θ generalized coordinates. While ω is non-

integrable, there is nevertheless a smooth and invertible transformation A(θ) suchthat Iω = A(θ)θ. Equation (B.2) on page 404 provides an explicit expression forA(θ) for one such choice of Euler angles for θ. It can be shown that A−∗γ∗ = ω.Since J is constant in the body frame, (4.44) reduces to the following simpler formof the equations of motion seen in (2.20):

J ω+ ωJω=A−∗f (4.47)

In contrast, if we choose to work with integrable generalized velocities, such as thetime derivatives of Euler angle generalized coordinates, the resulting equations ofmotion are significantly more complex than (4.47).

This example is an illustration of the use of quasi-velocities to simplify the dy-namical equations of motion. The price paid is that the kinematic relationship be-tween the generalized velocities and the time derivatives of the generalized coordi-nates is more complex.

Observe that

γθ4.45=[

A−∇θ(Aθ)]

θ= Aθ− Aθ = 0 (4.48)

However, γ itself vanishes if and only if the form A is a closed differentialform [119].

Exercise 4.9 Lagrangian equations of motion under coordinatetransformations.Let η = h(θ) denote a smooth, invertible nonlinear transformation of thegeneralized coordinates, and define the generalized velocities vector to be η. The


Lagrangian is now L(η, η) = L(θ, θ). Show that γ = 0 for this case, leading to thefollowing simplified version of the Lagrangian equations of motion

ddt∂L

∂η−∂L

∂η=A−∗T where A(θ)

�= ∇θh(θ) (4.49)

Observe that in Exercise 4.9, β = η = Aθ is the time-derivative of η and it ele-ments are therefore, not quasi-velocities. A simple instance of a closed differentialform A(θ) is when it is configuration independent and constant.

When the Lagrangian is of the form L = 12 θ

∗M(θ)θ as defined in (4.25), the

(4.44) equations of motion take the form:

M(θ)β+C(θ,β) =A−∗T

where M�= A−∗MA−1 and C

�= Mβ−A−∗∂L

∂θ+A−∗γ∗

∂L

∂β

(4.50)

The following exercise shows that the Mβ−2C generalized force is a non-workingforce similar to the non-working property derived earlier in Exercise 4.4 for M−2C.Thus this non-working property remains unaffected by the transformation of theLagrangian equations of motion using quasi-velocities.

Exercise 4.10 Non-working Mβ− 2C generalized force.Show that Mβ− 2C is a non-working generalized force, i.e., β∗(Mβ− 2C) = 0.

The following exercise looks at a special class of coordinate transformations thatdiagonalize and decouple the system equations of motion.

Exercise 4.11 Nonlinear diagonalizing coordinate transformations.Exercise 4.8 studied the transformation of the equations of motion from a change incoordinates. Now we study special coordinate transformations that diagonalize andsimplify the equations of motion in (4.50).

1. Let η = h(θ) ∈ RN denote a new set of generalized coordinates defined by aglobally invertible, smooth, and nonlinear coordinate transformation h(θ). Show

that if the matrix A(θ)�= ∇θh(θ) ∈ RN×N is such that M(θ) = A∗(θ)A(θ),

then the equations of motion are completely diagonalized in the (η, η) coordinatesand have the form

η= ζ (4.51)

where the new generalized forces vector ζ�= A−∗(θ)T ∈RN. In effect, the mass

matrix for the transformed system is the constant identity matrix, and the Cori-olis term is zero! In addition, the dynamics of the component η(k) generalizedcoordinates are completely decoupled and independent of each other.


2. We now relax the requirement that A(θ) is the gradient of a smooth transforma-tion. Instead, we only require thatA(θ) be such that M(θ)=A∗(θ)g(θ). With the

new velocity coordinates defined as β�= A(θ)θ ∈RN, show that the equations

of motion in the (θ,β) coordinates have the form

β+C(θ,β) = ζ, C(θ,β)�= −A−∗γ∗β (4.52)

While the mass matrix is once again the constant identity matrix, the Coriolisterm is no longer zero in this formulation. Since A(θ) is no longer required tobe the gradient of a function, the β velocity coordinates are in general quasi-velocities. This is another example of the use of quasi-velocities to simplify theequations of motion.

3. Show that while non-zero, the C(θ,β) Coriolis forces vector in (4.52) do no work.This is in contrast with the working nature of the C(θ, θ) Coriolis forces vector in(4.27).

The result in part (1) of this exercise is mainly of theoretical interest because theconditions on g(θ) are rarely satisfied by systems. These conditions are equivalentto requiring that the Riemannian symbols of the first kind, associated with the massmatrix, vanish [46] as discussed in more detail in Chap. 19. In the same chapter, spa-tial operator factorizations of the mass matrix are used to show that A(θ) satisfyingthe weaker conditions in part (2) always exists for tree-topology multibody systems.Thus, the (4.52) formulation of the diagonalized equations of motion always existsfor these systems.

Chapter 5Serial-Chain Dynamics

We now study the dynamics of serial-chain multibody systems and derive theirequations of motion. The approach builds upon the single rigid body dynamics andserial-chain kinematics developments in Chaps. 2 and 3. The component link-levelequations of motion are assembled into compact system-level operator versions ofthe equations of motion using the serial-chain mass matrix. Algorithms for the sys-tem inverse dynamics are also discussed.

5.1 Equations of Motion for a Typical Link

The first step in deriving the equations of motion for the serial-chain system is todevelop the free-body dynamics of the component links. To simplify the develop-ment, we continue to assume that the kth body frame, Bk, coincides with the Ok

hinge frame. Initially we assume that there are no external forces on the serial-chain;the only forces on the links are from their mutual interactions across the connectinghinges.

Let f(k) denote the spatial force of interaction between the (k+1)th and the kthlinks at Ok. As illustrated in Fig. 5.1, f(k) and −f(k) are equal and opposite spatialforces felt by the kth link at the Ok frame, and the (k+1)th link at the O

+k frame,

respectively, from the kth hinge coupling the two bodies. Thus, the total externalspatial forces on link k are the f(k) spatial force at the Ok frame and the −f(k− 1)

spatial force at the O+k−1 frame due to interactions across the kth and (k− 1)th

hinges, respectively. Using (1.47), the overall external force on the kth link refer-enced to frame Ok is therefore, [f(k)−φ(k,k− 1)f(k− 1)]. Using the equations ofmotion for a single rigid body from (2.28) on page 29, it follows that the equationsof motion of the kth link can be stated as:

f(k)−φ(k,k− 1)f(k− 1) =M(k)α(k)+b(k) (5.1)


76 5 Serial-Chain Dynamics

kth hinge

(k+1)th hinge

(k−1)th hingekth link(k+1)th link

f(k)−f(k)

f(k+1)

−f(k+1)

f(k−1)

−f(k−1)

Ok

Ok−1

Ok+1

O+k

O+k+1

O+k−1

Fig. 5.1 Illustration of the spatial interaction forces f(k) and f(k−1) on the kth andadjoining links

α(k) ∈ R6 and b(k) ∈ R6 denote the spatial acceleration and the velocity depen-dent gyroscopic force vectors for the kth link. Among the rigid body dynamicsoptions discussed in Chap. 2, one option is to define α(k) as the time derivative ofthe spatial velocity V(k) in the Bk frame. In this case, from Lemma 2.3 on page 29,we obtain

α(k)�=

dBkV(k)

dt=

dkV(k)

dtwith b(k)

2.28= V(k)M(k)V(k) (5.2)

An equally valid alternate option is to use inertial frame derivatives of V(k), inwhich case from Lemma 2.2 we would obtain

α(k)�=

dIV(k)

dtwith b(k)

2.26= ˜Vω(k)M(k)Vω(k) (5.3)

Based on either choice, (5.1) can be restated as

f(k) = φ(k,k− 1)f(k− 1)+M(k)α(k)+b(k) (5.4)

Equation (5.4) defines a recursive relationship between the f(k) and f(k− 1) spatialforces and the spatial acceleration of the kth link. We refer to it as the terminal-bodymodel for the component kth links.1

With f(0) = 0, we see that (5.4) holds for even the tip-body, which has no childbody. It thus, applies to all the links in the system. Observe that (5.4) is merelyestablishing the relationships that the inter-link spatial forces and spatial acceler-ations must satisfy. At this point, these quantities are only implicitly defined. The

1 We will study alternative composite-body and articulated-body dynamics models for the compo-nent links in Chap. 6.

5.1 Equations of Motion for a Typical Link 77

generalized force T(k) ∈ Rrv(k) for the kth hinge is the projection of the spatialforce f(k) onto the hinge axis given by

T(k) =H(k)f(k) (5.5)

5.1.1 Expression for the Spatial Acceleration α(k)

Equation (3.19b) established the following recursive relationship between the spatialvelocities of the kth and (k+ 1)th bodies:

V(k) = φ∗(k+ 1,k)V(k+ 1)+H∗

(k)θ(k) (5.6)

Equation (5.2) defines the spatial accelerations α(k) and α(k+ 1) of the kth and(k+ 1)th bodies as time derivatives of their spatial velocities with respect to theirown body frames. For this choice, differentiating (5.6) in the link frame Ok, andkeeping in mind that the relative spatial velocity of Ok with respect to Ok+1 isΔV(k), we obtain

α(k)5.2=

dkV(k)

dt1.12=

dk+1V(k)

dt− ˜ΔωV (k)V(k)

3.19b=

dk+1[

φ∗(k+ 1,k)V(k+ 1)+H∗(k)θ(k)]

dt− ˜ΔωV (k)V(k) (5.7)

That is,α(k) = φ∗

(k+ 1,k)α(k+ 1)+H∗(k)θ(k)+a(k) (5.8)

where the velocity dependent Coriolis spatial acceleration a(k) ∈R6 is defined as

a(k)�= − ˜ΔωV (k)V(k)+

dk+1φ∗(k+ 1,k)dt

V(k+ 1)+dk+1H

∗(k)dt

θ(k) (5.9)

While H∗(k) is constant in the body frame for most hinge types, it can depend onthe hinge generalized coordinates for more complex hinges. In this case, the timederivative of H∗(k) in (5.9) is related to its gradient with respect to the generalizedcoordinate of the hinge as follows:

dk+1H∗(k)

dt=[∇θ(k)(H

∗(k))

]

θ(k) (5.10)

Equation (5.8) has a recursive structure similar to that for link spatial velocities in(5.6). The explicit expression for the a(k) Coriolis acceleration is derived in thefollowing lemma.

Lemma 5.1 a(k) with body frame derivatives and Bk = Ok.For the case when body-frame derivatives in (5.2) are used, and Bk = Ok, theCoriolis acceleration, a(k), is given by the expression:

a(k) = ˜V(k)ΔV(k)− ΔV(k)ΔV(k)+dk+1H

∗(k)dt

θ(k) (5.11)


The explicit component level expression for a(k) is

a(k) = −

[

˜Δω(k)ω(k)

˜Δω(k)v(k)+ ˜Δv(k)ω(k+ 1)

]

+dk+1H

∗(k)dt

θ(k) (5.12)

Proof: When the kth hinge contains a prismatic component, the hinge to hingevector l(k+ 1,k) in (3.16) is not constant but instead depends on the configura-tion variable θ(k). The relative linear velocity across the hinge, Δv(k), is the timederivative of l(k+ 1,k) in the (k+ 1)th body frame, i.e.,

Δv(k) =dk+1 l(k+ 1,k)

dt(5.13)

As a consequence

dk+1φ(k+ 1,k)dt

3.16=

(

0 ˜Δv(k)

0 0

)

= Δv

V(k) (5.14)

Using (5.14) in (5.9), we have

a(k)5.14= − ˜ΔωV (k)V(k)− ˜ΔvV(k)V(k+ 1)+

dk+1H∗(k)

dtθ(k)

1.22= − ˜ΔV(k)V(k)+ ˜ΔvV(k) {V(k)−V(k+ 1)}+

dk+1H∗(k)

dtθ(k)

= ˜V(k)ΔV(k)+

[

0˜Δv(k)Δω(k)

]

+dk+1H

∗(k)dt

θ(k)

= ˜V(k)ΔV(k)+ ˜ΔvV(k)ΔωV (k)+dk+1H

∗(k)dt

θ(k)

1.24,1.36d= ˜V(k)ΔV(k)− ΔV(k)ΔV(k)+

dk+1H∗(k)

dtθ(k)

This establishes (5.11).Equation (5.12) is obtained by evaluating the components of the expression in

(5.11).It is worth emphasizing that the Coriolis acceleration expression in (5.11) is

based on the following assumptions:

1. The kth body spatial acceleration is defined as the time derivative of the bodyspatial velocity V(k) with respect to the kth body frame.

2. The kth body reference frame Bk coincides with the hinge frame Ok, i.e., thechoice for the kth body spatial velocity V(k) is the spatial velocity of the Ok

frame.3. The kth body spatial velocity V(k) is not an “inertially referenced” spatial

velocity.


As we will see in later exercises, other choices are possible and lead to differentexpressions for the Coriolis acceleration.

Remark 5.1 a(k) for simple hinges.While (5.11) provides the general expression for the link Coriolis acceleration spa-tial vector, it is usually simpler in practice. When the joint map matrix H∗(k) isconstant in the body frame, the last time derivative in (5.11) vanishes. Also, if thehinge is either purely prismatic or purely rotational then the second term vanishesas well. For these cases, a(k) simplifies to

a(k) = ˜V(k)ΔV(k) (5.15)

Examples of non-simple hinges include helical hinges, which contain both rotationaland prismatic components, and compound hinges, such as universal and gimbalhinges, where the direction of the axes depends on the configuration of the hinge.

We now look at specific examples of simple hinges such as the one degree offreedom rotary and prismatic hinges.

Example 5.1 a(k) for a 1 degree of freedom rotary pin hinge.When the kth hinge is the 1 degree of freedom rotary pin hinge in Fig. 3.2,H∗(k) =[

hω(k)

0

]

, where hω(k) ∈ R3 is the vector representation of the hinge axis in the

link frame. In this case ΔV(k) =

[

Δω(k)

0

]

. Since the joint axis is constant in the

body frame, it follows from (5.15) that

a(k) = ˜V(k)ΔV(k) =

[

ω(k)Δω(k)

v(k)Δω(k)

]

(5.16)

Example 5.2 a(k) for a 1 degree of freedom prismatic hinge.When the kth hinge is the 1 degree of freedom prismatic hinge in Fig. 3.2,H∗(k) =[

0hv(k)

]

. In this caseΔV(k) =

[

0Δv(k)

]

. Since the joint axis is constant in the body

frame, it follows from (5.15) that:

a(k) =

[

0

ω(k)Δv(k)

]

(5.17)


Exercise 5.1 a(k) for helical & cylindrical hinges.Derive the Coriolis vector a(k) for the helical and cylindrical hinges shown inFig. 3.2.

Equation (5.11) provides an expression for the link Coriolis acceleration a(k),with the assumption that the Bk body reference frame coincides with the Ok hingeframe. However, the link reference frame does not necessarily have to coincide withOk and may be a different frame of convenience, as discussed in Exercise 3.3 onpage 46. The Coriolis acceleration for such links is the subject of the next exercise.

Exercise 5.2 aB(k) with body frame derivatives but Bk �= Ok.For the case when body-frame derivatives in (5.2) are used, but Bk �= Ok, show thatthe Coriolis acceleration, aB(k), is given by the expression:

aB(k) = φ∗(Ok,Bk)a(k)

5.11= φ∗

(Ok,Bk)

[

˜V(Ok)ΔV(k)− ΔV(k)ΔV(k)+dk+1H

∗(k)dt

θ(k)

]

where a(k) is defined in (5.11). The direct expression for aB(k) is:

aB(k) = ˜V(k)ΔB

V(k)− ΔV(k)ΔV(k)+φ∗(Ok,Bk)

dk+1H∗(k)

dtθ(k) (5.18)

where ΔB

V(k) is the hinge relative spatial velocity referenced to the kth body frameBk defined in (3.22).

Yet another option is to define the spatial acceleration of the link as the timederivative of the spatial velocity of the link with respect to the inertial frame. Thisoption is the subject of the next exercise.

Exercise 5.3 aI(k) with inertial frame derivatives.For the case when inertial-frame derivatives described in (5.3) are used forspatial accelerations, show that the Coriolis acceleration, aI(k), is given by theexpression:

aI(k) = a(k)+ V(k)V(k)− V(k+ 1)V(k+ 1)

= ˜Vω(k+ 1)[

Vv(k)−Vv(k+ 1)+ΔV(k)]

+dk+1H

∗(k)dt

θ(k) (5.19)

where a(k) is as defined in (5.11). The component level expression for aI(k) is:


aI(k) = a(k)+

[

0

ω(k)v(k)− ω(k+ 1)v(k+ 1)

]

(5.20a)

=

[

ω(k)Δω(k)

ω(k+ 1)[

v(k)−v(k+ 1)+Δv(k)]

]

+dk+1H

∗(k)dt

θ(k) (5.20b)

5.1.2 Overall Equations of Motion

Taken together, (5.4)–(5.6) and (5.8), define the following link-level equations ofmotion for the serial-chain system:

V(k) = φ∗(k+ 1,k)V(k+ 1)+H∗

(k)θ(k)

α(k) = φ∗(k+ 1,k)α(k+ 1)+H∗

(k)θ(k)+a(k)

f(k) = φ(k,k− 1)f(k− 1)+M(k)α(k)+b(k)

T(k) = H(k)f(k) (5.21)

These equations of motion fully describe the relationship between the generalizedaccelerations, θ(k), and the corresponding generalized forces, T(k), given a specific(θ, θ) state of the system.

Now that we have the component level equations of motion, we can proceed todevelop system-level operator versions of the equations of motion. Analogous tothe stacked vector definition of V and θ in Sect. 3.4, define the additional stackedvectors:

T�= col

{T(k)

}nk=1

∈RN

f�= col

{f(k)

}nk=1

∈R6n α�= col

{α(k)

}nk=1

∈R6n

a�= col

{a(k)

}nk=1

∈R6n b�= col

{b(k)

}nk=1

∈R6n

Using these stacked vectors, the component-level expressions in (5.21) can be as-sembled into the following equivalent system-level operator expressions:

V = E∗φV+H∗θ

α = E∗φα+H∗θ+a

f = Eφf+Mα+b

T = Hf (5.22)

Using φ= (I−Eφ)−1 from (3.36), these implicit expressions can be converted intothe following explicit ones:


V = φ∗H∗ θα = φ∗

(H∗ θ+a)

f = φ(Mα+b)

T = Hf (5.23)

Combining the expressions in (5.23) we obtain

T = M(θ)θ+C(θ, θ) (5.24)

where

M(θ) =HφMφ∗H∗ ∈RN×N and C(θ, θ)�= Hφ(Mφ∗

a+b) ∈RN (5.25)

Recall that the above expression for the M mass matrix is the same as the Newton–Euler Operator Factorization expression derived in (4.6) on page 58. C consists ofthe velocity dependent Coriolis and gyroscopic hinge forces.

The expression for C in (5.25) is different from the one derived in (4.27) usingLagrangian techniques. Exercise 18.9 on page 368 shows that the two expressionsare in fact equivalent. References [94, 96, 111, 132, 138, 159, 160, 163] discuss otherapproaches to formulating the dynamics of multibody systems, with [9, 32, 51, 66,166] focusing specifically on dynamics for the robotics context.

By differentiating the expression for V in (5.23) and comparing it to the expres-sion for α, we can conclude that

dφ∗H∗

dtθ = φ∗

a (5.26)

The time derivative above is assumed to be consistent with the time derivative usedto define α from V.

Remark 5.2 Body dynamics invariance across Newtonian frames.Remark 2.3 on page 30 discussed the invariance of the dynamics of a single bodyin Newtonian frames. This fact extends to multibody systems as well. The dynamicsof a multibody system are invariant across Newtonian frames though not acrossnon-Newtonian ones.

Observe that the velocity values enter into the dynamics equations of motion onlythrough the a(k) and b(k) terms and any impact on the equations of motion has tohappen through these quantities.

Switching to a different Newtonian frame can result in a δv change in the linearvelocity of the base-body. This change results in a similar additive change to thelinear velocity of each body in the system, so that v(k) becomes v(k)+δv. Focusingon the inertial frame derivatives form of the equations of motion, we see that δvhas no effect on the expression for aI(k) in (5.19) because it is eliminated fromthe v(k) − v(k+ 1) sub-expression. Also, the expression for the bI(k) gyroscopicforce in (2.23) does not contain the linear velocity term, and hence, δv has no effect


on it either. Thus, the dynamics equations of motion are independent of the linearvelocity of the base-body, and hence, they are unaffected by the specific choice of aNewtonian frame.

On the other hand, both the Coriolis acceleration and gyroscopic force expres-sions involve the body angular velocities. Any changes in the angular velocity ofthe base-body – due to the use of non-Newtonian rotating frames – ripple throughthe angular velocities of all the bodies, changing the values of these Coriolis andgyroscopic terms and, in effect, changing the dynamics of the system.

5.1.3 Spatial Operators with Body Frame Derivatives but Bk �= Ok

Equation (5.23) provides expressions for the spatial operators φ, H etc., with theassumption that the body reference frame Bk coincides with the hinge frame Ok.However, as we have seen in Sect. 3.3.3 on page 45, the link reference frames donot necessarily have to coincide with the hinge frame. The EφB

operator for thisformulation contains φ(Bk+1,Bk) entries instead of φ(Ok+1,Ok) along the sub-diagonal. We now derive expressions for the φB, HB and MB operators for thisalternate formulation in terms of the component level terms in (5.23).

From (3.23) and (3.22) we have

φ(Bk+1,Bk) = φ(Bk+1,Ok+1) φ(Ok+1,Ok) φ(Ok,Bk)

H∗B(k)

3.22= φ∗

(Ok,Bk)H∗(k)

MB(k)�= φ(Bk,Ok)M(Ok)φ

∗(Bk,Ok) (5.27)

MB(k) is the spatial inertia of the kth body, referenced to its Bk body frame. Definethe block-diagonal operator

ΔB/O

�= diag

{φ(Bk,Ok)

}nk=1

∈R6n×6n (5.28)

It is easy to verify that EφBand Eφ are related via the following similarity transfor-

mation2

EφB

5.27= ΔB/OEφΔ

−1B/O

(5.29)

It thus, follows that

φB

�= (I−EφB

)−1

= (I−ΔB/OEφΔ−1B/O

)−1

= ΔB/O(I−Eφ)−1Δ−1

B/O= ΔB/OφΔ

−1B/O

(5.30)

2 With T being an invertible matrix and A another matrix, the TAT−1 matrix is referred to as asimilarity transformation ofA.


Thus, φB is also related to Eφ by the same similarity transformation. Also,

HB

�= diag

{HB(k)

}nk=1

=HΔ−1B/O

MB

�= diag

{MB(k)

}nk=1

= ΔB/OMΔ∗B/O

(5.31)

While we now have expressions for the new spatial operators, it is noteworthy thatthe mass matrix remains invariant in this new formulation. This follows from:

HBφBMBφ∗BH

∗B

5.30, 5.31= (HΔ−1

B/O) · (ΔB/OφΔ

−1B/O

)(ΔB/OMΔ∗B/O

)

· ((Δ−1B/O

)∗φ∗Δ∗

B/O) · ((Δ−1

B/O)∗H∗

)

= HφMφ∗H∗ 5.25= M

This implies that even though the internal structure and formulation of the spatialoperators is different, the mass matrix remains unaffected.

The same is true for the C generalized forces vector, whose value remains un-affected by the specific choices of link frame locations and derivative frames. Thisis true, despite the values of α, a and b etc., depending on the frame choices. Theparamount requirement, for a valid definition of the equations of motion, is that thecomponent terms be defined consistently for a specific selection of link frame lo-cations (e.g. hinge or non-hinge frame) and derivative frames (e.g. body frame orinertial frame). Thus, the a definition in Lemma 5.1 applies to the frame choices in(5.2), while the definition in Exercise 5.3 applies to the frame choices in (5.3). Otherfactors, including computational considerations, may influence the selection of theframes. Assuming such consistency, and in the spirit of coordinate-free notation, wewill henceforth refer to, and use α, a and b etc., in the equations of motion withoutspecifying the associated frames – except in situations where it is necessary to do so.As a reference, Table 5.1 summarizes the a(k) and b(k) expressions for different

Table 5.1 The body Coriolis acceleration and gyroscopic force expressions for the differentchoices of link spatial velocities

Link spatial velocity α(k) Derivative frame Coriolis accel a(k) Gyroscopic force b(k)

V(Ok) Bk (5.11) (2.28)

V(Bk) Bk (5.18) (2.28)

V(Ok) I (5.19) (2.26)

VI(k) I (5.55) (2.38)

5.2 Inclusion of External Forces and Gravity 85

link spatial velocity and derivative options. Note that the last row for the inertiallyreferenced link spatial velocity VI(k) is discussed later in Sect. 5.4.

Section 8.5 on page 147 discusses generalizations of the (5.29) and (5.30) simi-larity transformations of spatial operators in a broader and more general context.

5.2 Inclusion of External Forces and Gravity

We now study the effect of external spatial forces being applied at the nodes on theequations of motion.

5.2.1 Inclusion of External Forces

Our development of the equations of motion so far has assumed that the only forceson the links are from the interaction forces among the links and has not taken intoaccount any external forces on the system. Thus, the free-body equations of motionfor the kth link in (5.1) only involve the f(k) and f(k− 1) inter-link forces. We nowlook at extensions to handle external forces on the system.

Let us assume that external spatial forces are being applied at nodes on the bodiesin the serial-chain. Adopting the notation for nodes from Sect. 3.6 on page 53, Oikdenotes the ith such node on the kth link. Let fiext(k) denote the external spatialforce being applied at the O

ik node. This spatial force is effectivelyφ(k,Oik)f

iext(k)

at the Bk body frame. Summing up all such external spatial forces on the kth link,the free-body equations of motion for the link in (5.1) are modified as follows:

f(k)−φ(k,k− 1)f(k− 1)+∑i

φ(Bk,Oik)fiext(k) =M(k)α(k)+b(k) (5.32)

Assuming that there are nnd such external forces on the system, analogous to(3.49), define the 6nnd dimensional stacked vector, fext of all external forces as

fext = col{fiext(k)

}∈R6nnd (5.33)

Using (5.32), the equivalent operator-level expression for (5.32) is

f = Eφf−Bfext+Mα+b (5.34)

where B is the pick-off spatial operator for the nodes from (3.51). Equation (5.34)leads to the following non-implicit expression that replaces the version in (5.23):

f = φ(Mα+b−Bfext) (5.35)


The overall equations of motion in (5.24) take the new form:

T = Mθ+C−HφBfext3.53= Mθ+C−J∗fext (5.36)

with J = B∗φ∗H∗ being the Jacobian from (3.53) for the external force nodes.Thus, in the operator formulation, the external forces can be incorporated into theequations of motion by including the additional J∗fext term. One option is to absorbthe term within the Coriolis generalized forces vector as follows

C(θ, θ) =Hφ[Mφ∗a+b−Bfext] (5.37)

and continue to work with the T = Mθ+C expression from (5.24).

5.2.2 Compensating for External Forces

In control applications, sometimes there is a need to apply additional generalizedforces at the hinge actuators to neutralize the effect of external disturbances perturb-ing the motion of the system. To compensate for such fext external forces, (5.36)shows that adding the additional J∗fext term to the nominal T generalized forceneutralizes the effect of the external forces on the system dynamics. In other words,if T represents the generalized forces to be applied in the absence of external forces,then T−δT , is the required generalized forces that will result in the same motion inthe presence of fext external forces, where

δT�= J∗fext =HφBfext (5.38)

A recursive tip-to-base procedure for computing δT is described in Algorithm 5.1.

Algorithm 5.1 Compensating generalized forces for external spatial forces⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

x(0) = 0

for k = 1 · · ·nx(k) = φ(k,k− 1)x(k− 1)+

∑i

φ(Bk,Oik)fiext(k)

δT(k) =H(k)x(k)

end loop

5.2 Inclusion of External Forces and Gravity 87

5.2.3 Inclusion of Gravitational Forces

Extensions to the equations of motion in (5.23) to include the effect of a uniformgravity field is the subject of the following exercise.

Exercise 5.4 Inclusion of gravitational forces.

Let g =

[

0

gl

]

denote the uniform gravitational spatial acceleration vector, with gl

denoting the linear gravitational acceleration.

−g pseudoaccelerationat the base

Gravitational forceon each body inthe system

Gravitationalpseudo-accelerationon just the base-body

M(k)g

M(k+1)g

Fig. 5.2 Alternate ways of handling gravitational forces on a system

1. The effect of gravity can be handled by applying a pseudo-acceleration of g to thebase-body. Show that this is equivalent to replacing a by a+E∗g in the expressionfor C in (5.25), so that

C(θ, θ) =Hφ[

Mφ∗(a+E∗g)+b

]

(5.39)

E is the pick-off spatial operator defined in (4.14).2. An alternative approach is to add an additional gravitational spatial force of

−M(k)g to the right-hand side of the free-body equation in (5.1). Verify that thisleads to the following expression for the Coriolis forces vector:

C(θ, θ) =Hφ[

M(φ∗a+ E∗g)+b

]

, where E�= [I6, · · ·I6,I6] ∈R6×6n (5.40)

3. Show that the two expressions for C in (5.39) and (5.40) are equivalent, i.e., showthat


φ∗E∗g = E∗g (5.41)

(Hint: That g only has a non-zero linear acceleration component plays a key rolein the proof.)

5.3 Inverse Dynamics of Serial-Chains

Two topics of considerable interest for multibody system dynamics are the inverseand forward dynamics problems.

• The inverse dynamics problem consists of computing the T generalized forcesrequired to obtain a desired θ multibody generalized acceleration for a given(θ, θ) state of the system. Thus, it requires the evaluation of T = Mθ+C to obtainT. The inverse dynamics problem appears in the context of multi-link robot con-trol, where its solution is used to generate real-time actuator force commandsfor hinge actuators, to obtain desired system motion trajectory profiles [107].Efficient algorithms for solving the system inverse dynamics problem are neededfor use in real-time control loops. We examine computational algorithms for theinverse dynamics problem in the following section.

• The forward dynamics problem is the converse. It involves computing the θgeneralized accelerations that result from the application of a specified T gener-alized forces for a given (θ, θ) state of the system. Thus, it requires the evaluationof θ = M−1(T −C) for θ. The solution to the forward dynamics problem is re-quired for the simulation of the system dynamics. The input generalized forcesare obtained from actuator commands and environmental interactions. The com-puted generalized accelerations are numerically integrated to predict the evolu-tion of the state of the system. Simulation speed and accuracy is the driver fordeveloping efficient and numerically sound algorithms for solving the forwarddynamics problem. Low-cost solution techniques for the forward dynamics ofserial-chain systems are discussed in Chap. 7.

5.3.1 Newton–Euler Inverse Dynamics Algorithm

It would appear that the evaluation of T = Mθ+C for the inverse dynamics solu-tion requires the explicit computation of M and C. Such computations would be ofat least O(N2) complexity since it involves the computation of a matrix of size N.We can do much better than this using ideas from Sect. 3.5. There we encounteredthe feature that spatial operator expressions can be evaluated using low order recur-sive algorithms. We take this approach to develop the O(N) Newton–Euler inversedynamics algorithm.

5.3 Inverse Dynamics of Serial-Chains 89

For the inverse dynamics problem, the hinge accelerations θ are assumed tobe known. The spatial operator equations of motion in (5.21) map easily intoAlgorithm 5.2, which is referred to as the Newton–Euler inverse dynamicsalgorithm for computing the inverse dynamics of the serial-chain. The structure ofthis algorithm is illustrated in Fig. 5.3. This algorithm is an O(N) computationalprocedure involving a base-to-tip recursion sequence to compute the spatial veloc-ities and accelerations, followed by a tip-to-base recursion to compute the hingeforces.

Algorithm 5.2 Newton–Euler inverse dynamics algorithm⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

V(n+ 1) = 0, α(n+ 1) = 0

for k = n · · ·1V(k) = φ∗

(k+ 1,k)V(k+ 1)+H∗(k)θ(k)

α(k) = φ∗(k+ 1,k)α(k+ 1)+H∗

(k)θ(k)+a(k)

end loop

⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

f(0) = 0

for k = 1 · · ·nf(k) = φ(k,k− 1)f(k− 1)+M(k)α(k)+b(k)

T(k) =H(k)f(k)

end loop

(5.42)

The first base-to-tip recursion in (5.42) is an unrolling of the operator expressionsfor V and α in (5.23), and an application of Lemma 3.2 to convert the operator ex-pressions into base-to-tip recursive steps. Similarly, the latter tip-to-base recursionin (5.42) follows from the operator expressions for f and T in (5.23), and an appli-cation of Lemma 3.1 to convert the operator expressions into tip-to-base recursivesteps.

Except for the use of spatial notation, Algorithm 5.2 is essentially the inversedynamics algorithm from Luh et al. [120]. Based on (5.32) and (5.40), the followingrevision to the f(k) expression in (5.42) allows us to include the effects of externalforces and gravity into the inverse dynamics algorithms:

f(k) = φ(k,k− 1)f(k− 1)+M(k)(

α(k)+g)

+b(k)

−∑i

φ(Bk,Oik)fiext(k)

(5.43)

The Newton–Euler inverse dynamics algorithm accomplishes the computation ofT = Mθ+ C without requiring the explicit computation of M or C. In fact, nei-ther the explicit computation of the operator φ nor of any of the operator-operator


Step Step 2 Step 3

Base to tipkinematicssweep

Base to tipvelocities andaccelerationssweep

Tip to basesweep forforces

1

1 1 1

k k k

n n n

Fig. 5.3 The structure of the Newton–Euler inverse dynamics algorithm

products was required. This again exemplifies the property whereby the evaluationof operator expressions rarely requires the explicit (and expensive) computation ofthe operators, but can typically be accomplished by recursive computations at thecomponent level. References [19, 54–56, 114] describe other approaches, includingparallel algorithms, for solving the inverse dynamics problem.

5.3.2 Computing the Mass Matrix Using Inverse Dynamics

Algorithm 4.2 on page 64 describes a procedure for computing the mass matrixusing composite body inertias. We now describe an alternative algorithm for com-puting the mass matrix. using theO(N) Newton–Euler inverse dynamics algorithm.

First the generalized velocities vector θ is set to zero, so that the C vector be-comes zero. The jth column of M can be computed by running the inverse dynamicsalgorithm with all the hinge accelerations set to zero, except the jth hinge accelera-tion which is set to 1. The resulting T vector is the jth column of M. Repeating thisfor all j, from 1 through n, yields all n columns of M. This process is illustrated inFig. 5.4.

We can improve upon this algorithm by realizing that, due to its symmetry, onlythe upper-triangular half of M needs to be explicitly computed. To compute theupper part of the jth column, we can use the smaller manipulator made up of justlinks 1 through j, and run the inverse dynamics with the above set of accelerations(ignoring the ones for k > j). The resulting generalized forces vector yields the jthcolumn elements of M above the diagonal. Repeating this for all j from 1 to n yieldsthe upper-triangular half of M, and, in effect, all the elements of M.

5.3 Inverse Dynamics of Serial-Chains 91

.......................................

Column 1 Columnk Columnn

Step 1 Stepk Stepn

M =

Inverse dynamicswith θ(1) = 1

Inverse dynamicswith θ(j) = 1

Inverse dynamicswith θ(n) = 1

Fig. 5.4 Inverse dynamics based computation of the columns of the M mass matrix

Since the O(N) steps are repeated n times, the computational cost of thisalgorithm remains O(N2), as for the composite body inertia based Algorithm 4.2.However this procedure is decoupled in that the inverse dynamics sweeps for eachof the columns can be run independently in parallel. Thus, the cost of this proce-dure reduces to O(N) when n processors are used. However, for single processorimplementations, the composite body inertia algorithm remains considerably moreefficient than this inverse dynamics based procedure. Additional discussion of thecomputational issues can be found in Walker and Orin [179].

5.3.3 Composite Rigid Body Inertias Based Inverse Dynamics

While the discussion of composite rigid body inertias in Sect. 4.1.2 on page 59 wasmotivated by the goal of computing the mass matrix, in the following exercise, welook at how the composite body inertias can be used to develop an inverse dynamicsproblem.

Exercise 5.5 Inverse dynamics using composite body inertias.Equation (5.23) described the spatial forces expression f = φ(Mα+ b). Here wewill look at an alternative expression for f using the R(k) composite body inertias.

1. Show that f can be expressed in the form

f = Rα+y where y�= φ

[

b+EφR(H∗ θ+a)]

(5.44)


2. Show that the elements of y can be computed using the following tip-to-baserecursion: ⎧⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎩

y+(0) = 0

for k = 1 · · ·ny(k) = φ(k,k− 1)y+

(k− 1)+b(k)

y+(k) = y(k)+R(k)

[

H∗(k)θ(k)+a(k)

]

end loop

(5.45)

Equation (5.44) may be thought of as a composite-body model that is an alterna-tive to the terminal-body model for the serial-chain in the sense that, at the compo-nent link level, the following relationship holds:

f(k) = R(k)α(k)+y(k) (5.46)

The physical interpretation of this model is as follows. f(k) is the force that the(k+ 1)th link exerts on the outboard links at Ok and it produces a spatial accel-eration of α(k) at Ok. If the outboard links did indeed form a composite body,their composite spatial inertia at Ok would be R(k), and the R(k)α(k) D’Alembertforce would be equal to f(k). However, the links k through 1 in the serial-chain donot form a composite body, and the deviation from the composite body model at Ok

is because the hinge accelerations θ(j) �= 0 for j ∈ (k− 1) · · ·1. The extra correctionforce y(k) compensates for this. By this argument,y(k) should be a function of onlythe hinge accelerations θ(j) for j ∈ (k− 1) · · ·1. This is indeed the case. The spatialforce decomposition in (5.44) also forms the basis for Algorithm 5.3 for solvingthe inverse dynamics of the system. The structure of this algorithm is illustrated inFig. 5.5.

Algorithm 5.3 Composite rigid body inertias based inverse dynamics1. Compute the kinematics of the system in a base-to-tip recursion.2. Use the base-to-tip recursion part of Algorithm 5.2 on page 89 to compute theα(k) link spatial accelerations and the a(k) and b(k) Coriolis and gyroscopicterms. This recursion can be combined with the one in (1).

3. Use (5.45) and Algorithm 4.1 on page 60 in a combined tip-to-base recursion tocompute the R(k) composite body inertia and y(k) terms. Compute the hingegeneralized force for the kth hinge using the relationship

T(k)5.21= H(k)f(k)

5.44= H(k)

[

R(k)α(k)+y(k)]

The (5.46) decomposition is in contrast with the terminal-body decompositionin (5.4), wherein the correction term φ(k,k− 1)f(k− 1) is a function of all thehinge accelerations. The notion of alternate models is useful in multibody dynamics


Step 3

Base-to-tipkinematicssweep

Base-to-tipvelocities andaccelerationssweep

Tip-to-basesweep for CRBinertias andforces

1 11

k k k

n n n

Step 1 Step 2

Fig. 5.5 Composite rigid body inertias based inverse dynamics

analysis. The composite-body model is appropriate for developing efficient algo-rithms for the inverse dynamics problem. As will be seen in Chap. 6, an articulated-body model, and its accompanying decomposition, form the basis for a recursiveforward dynamics algorithm.

5.4 Equations of Motion with an Inertially Fixed VelocityReference Frame

Exercise 2.14 on page 32 studied the equations of motion of a single rigid bodyusing an inertially fixed velocity reference frame, I. Exercise 3.4 on page 47 furtherdeveloped the kinematics for a serial-chain using an inertially fixed velocity refer-ence frame. We now extend this to develop the equations of motion for a inertiallyfixed velocity reference frame formulation.

Recall from (2.32) that the spatial inertia, MI(k), of the kth link about frame I

is defined as

MI(k) = φ(I,Ok)M(k)φ∗(I,Ok) =

(

JI(k) m(k) pI(k)

−m(k) pI(k) m(k)I3

)

(5.47)

Also, recall from (3.24) on page 47, that the inertially referenced spatial velocity ofthe kth link, VI(k), is defined by

VI(k) = φ∗(Ok,I)V(Ok) =

[

ω(k)

vI(k)

]

(5.48)


with ω(k) and vI(k) denoting its angular and linear velocity components. Asdiscussed in (3.26) on page 47, the VI(k) link spatial velocities satisfy the recursiverelationship

VI(k) = VI(k+ 1)+ΔI

V(k) = VI(k+ 1)+H∗I (k)θ(k) (5.49)

With fI(k)�= φ(I,Ok)f(Ok) denoting the spatial interaction force between the

(k+ 1)th and the kth links (referred to frame I), the total external spatial force onthe kth link is [fI(k)− fI(k− 1)]. From (2.37) on page 33, the equations of motionfor the kth link are:

fI(k) = fI(k− 1)+MI(k)αI(k)+bI(k) (5.50)

where

αI(k)�=

dIVI(k)

dtand bI(k)

2.38= VI(k)MI(k)VI(k) (5.51)

Exercise 5.6 Expression for H∗I(k).

Show that

H∗I (k)

�=

dIH∗I(k)

dt= ˜VI(k)H

∗I (k)+φ∗

(Ok,I)dk+1H

∗(k)dt

(5.52)

In most situations, dk+1H∗(k)

dt ≡ 0, and then (5.52) simplifies to

HI(k) = ˜VI(k)H∗I (k) (5.53)

Differentiating (5.49), it follows that the link spatial accelerations α(k) satisfythe recursive relationship

αI(k) = αI(k+ 1)+H∗I (k)θ(k)+aI(k) (5.54)

with

aI(k)�= H

∗I (k)θ(k)

5.52= ˜VI(k)H

∗I (k)θ(k)+φ∗

(Ok,I)dk+1H

∗(k)dt

3.25= ˜VI(k)Δ

I

V(k)+φ∗(Ok,I)

dk+1H∗(k)

dt(5.55)

The hinge torque at the kth hinge is

T(k) =HI(k)fI(k) (5.56)

Together, the relationships in (5.49), (5.50), (5.54), and (5.56) define the followingequations of motion for the serial-chain:


VI(k) = VI(k+ 1)+H∗I (k)θ(k)

αI(k) = αI(k+ 1)+H∗I (k)θ(k)+ H

∗I (k)θ(k)

fI(k) = fI(k− 1)+MI(k)αI(k)+bI(k)

T(k) = HI(k)fI(k) (5.57)

Define the block diagonal spatial operators HI = diag{HI(k)

}, MI =

diag{MI(k)

}, with EI and φI as

EI =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 . . . . . . . . . 0

I 0 · · · · · · ...

0 I 0 · · · ......

. . .. . .

...

0 . . . . . . I 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

∈R6n×6n, and φI =

⎛

⎜

⎜

⎜

⎜

⎜

⎝

I . . . . . . 0

I I...

.... . .

...

I . . . . . . I

⎞

⎟

⎟

⎟

⎟

⎟

⎠

∈R6n×6n

(5.58)Analogous to (3.36), we have

φI = [I−EI]−1, and EIφI = φIEI = φI − I

�= φI (5.59)

It is noteworthy that, for the inertially referenced case, φI is independent of the

system configuration, though now H∗I

and MI are configuration dependent. Definethe overall link spatial velocity vector VI for the system as

VI

�= col

{VI(k)

}nk=1

Using a similar procedure, define the αI, fI etc., stacked vectors. Using these spatialoperators and the stacked vectors, (5.57) can be re-expressed in the form,

VI = φ∗IH

∗I θ

αI = φ∗I [H∗

I θ+ H∗I θ] = φ∗

I

d[H∗Iθ]

dt

x = MIαI + MIVI =d[MIφ

∗IH∗

Iθ]

dt=

dMIVI

dt(5.60)

fI = φIx= φI

d[MIφ∗IH∗

Iθ]

dt

T = HIfI =HIφI

d[MIφ∗IH∗

Iθ]

dt= Mθ+C


where

M(θ)�= HIφIMIφ

∗IH

∗I

C(θ, θ)�= HIφI

d[MIφ∗IH∗

I]

dtθ=HIφI

[

MIVI +MIφ∗I H

∗I θ]

(5.61)

5.4.1 Relationship Between φI and φ

Along the lines of Sect. 5.1.3, we examine the relationship between the operatorsfor the regular and inertially referenced equations of motion. We have

I = φI(k+ 1,k) = φ(I,Ok+1)φ(Ok+1,Ok)φ(Ok,I) (5.62)

Define the block-diagonal operator

ΔI/O

�= diag

{φ(I,Ok)

}nk=1

∈R6n×6n (5.63)

Then, from (5.62), EI and Eφ are related by the following similarity transformation:

EI = ΔI/OEφΔ−1I/O

(5.64)

Thus, φI is related to φ as follows:

φI

5.59= (I−EI)

−1 5.64= (I−ΔI/OEφΔ

−1I/O

)−1

= ΔI/O(I−Eφ)−1Δ−1

I/O= ΔI/OφΔ

−1I/O

(5.65)

Thus, φI and φ are also related by the same similarity transformation. It is straight-forward to also see that

HI =HΔ−1I/O

MI = ΔI/OMΔ∗I/O

(5.66)

Even though the internal structure of these spatial operators is different, the massmatrix remains unchanged in the new formulation, since

HIφIMIφ∗IHI

∗ 5.65,5.66= (HΔ−1

I/O) · (ΔI/OφΔ

−1I/O

) · (ΔI/OMΔ∗I/O

)

· (ΔI/OφΔ−1I/O

)∗ · (HΔ−1

I/O)∗

=HφMφ∗H∗ 5.25= M

Chapter 6Articulated Body Model for Serial Chains

This chapter introduces articulated body models for the component links in aserial-chain system. This model is an alternative to the terminal and composite bodymodels discussed earlier. While the composite body model is appropriate for theinverse dynamics problem, the articulated body model is better suited for the for-ward dynamics problem.

6.1 Alternate Models for Multibody Systems

Chapter 5 discusses two alternative models for the dynamics of the componentlinks in the serial-chain system. They are the terminal body and the composite bodymodels. Each of these models uses different effective inertias to define the systemdynamics as viewed from the kth link in the system. Since these models do not bythemselves fully capture the articulated dynamics of the system, they require cor-rection force terms to compensate for the gap between the reference model and thecorrect system dynamics [71].

6.1.1 Terminal Body Model

In the terminal body model, the kth link is viewed as a terminal body and theconnection to its children bodies is initially ignored. For this case, the effectiveinertia at the kth link is the familiar spatial inertia of the kth link. Thus, for thisterminal body reference model, the force/acceleration relationship at the kth link isdefined as:

f(k) =M(k)α(k) (6.1)

The terminal body reference model is only correct for truly terminal bodies. For non-terminal bodies, the coupling to the children bodies is accounted for by the addition


98 6 Articulated Body Model for Serial Chains

of the “correction” force φ(k,k− 1)f(k− 1), so that the correct force/accelerationrelationship for the kth link is given by

f(k) =M(k)α(k)+φ(k,k− 1)f(k− 1) (6.2)

This expression is the same as the one in the equations of motion in (5.21). Thus,the M(k)α(k) inertia/acceleration part reflects the “predicted” dynamics based onthe reference model, while theφ(k,k− 1)f(k− 1) spatial force compensates for themismatch between the predicted and the actual dynamics. The φ(k,k− 1)f(k− 1)

correction force is a function of the hinge forces and accelerations of the inboard, aswell as the outboard, bodies in the system.

6.1.2 Composite Body Model

In the composite body model, the links 1 through k are viewed as forming acomposite body by locking the 1 through k− 1 hinges. The articulation of theoutboard hinges is initially ignored. The effective inertia at the kth link, then,is the R(k) composite body inertia. This reference model leads to the followingforce/acceleration relationship at the kth link:

f(k) = R(k)α(k) (6.3)

In other words, this reference model assumes that all the hinges outboard of thekth link are frozen and the sub-system can be treated as a rigid body with R(k)

spatial inertia. Since the outboard hinges are not frozen in reality, a correction termis needed to account for the presence of non-zero accelerations at the outboard 1through k− 1 hinges. As seen in (5.46) on page 92, the spatial force y(k) providesthe necessary correction term, so that the correct force/acceleration relationship atthe kth link is given by

f(k) = R(k)α(k)+y(k) (6.4)

The force/acceleration relationship in (6.4) at the kth link is an alternative to theone in (6.2). The representations differ in both the spatial inertia term and the cor-rection force term. The R(k) spatial inertia term is a function of the spatial in-ertias of all the links 1 through k, in contrast with M(k), which depends on thespatial inertia of the kth link alone. The y(k) residual force term is simpler thanthe earlier φ(k,k− 1)f(k− 1) term in that it depends only on the outboard hingeaccelerations θ(j), j < k, as discussed in Sect. 5.3.3. This is in contrast with theφ(k,k− 1)f(k− 1) correction force for the terminal body model, which is a func-tion of all hinge accelerations. When the outboard hinge accelerations are indeedzero, y(k) = 0, and the force/acceleration relationship reduces to the one in (6.3).

6.1 Alternate Models for Multibody Systems 99

6.1.3 Articulated Body Model

We now introduce another dynamics model, referred to as the articulated bodymodel for serial-chain systems. In this model, it is assumed that all the outboard1 through k− 1 hinges are completely free with zero hinge forces, i.e., T(j) = 0,for j < k. The prevalence of non-zero generalized forces at these hinges is initiallyignored. This is in contrast with the composite body model, where the outboardhinges are assumed to be locked with zero hinge accelerations. We will establishthat this reference model leads to the following force/acceleration relationship atthe kth link:

f(k) = P(k)α(k) (6.5)

and that the P(k) inertia term in (6.5), referred to as the articulated body inertia,is well defined.

Since, in reality, the outboard 1 through (k−1) hinges have non-zero generalizedforces, we will further show that there is a well-defined correction spatial force, z(k),that compensates for these non-zero hinge forces. The correct force/accelerationrelationship has the form:

f(k) = P(k)α(k)+ z(k) (6.6)

In the articulated body model, P(k) plays the role of the inertia, while z(k) playsthe role of the correction force term. We will see that the z(k) spatial force dependsonly upon the outboard hinge forces, and is zero when all the outboard hinge forcesare indeed zero. Figure 6.1 illustrates the three different reference models and theircorresponding force decompositions at the kth link.

kth link

α(k) spatialacceleration

f(k) f(k−1)

Terminal-body model: f(k) = M(k)α(k)+f(k−1)Composite-body model: f(k) = R(k)α(k)+y(k)Articulated-body model:f(k) = P(k)α(k)+z(k)

Fig. 6.1 The terminal-body, composite-body, and articulated-body model decompo-sitions of the f(k) spatial force at the kth link

The concept of articulated body inertias for a serial rigid multibody systemwas originally introduced in Featherstone [47], to develop the remarkable O(N)

AB algorithm for solving the forward dynamics problem for serial-chain systems.


Independently, Rodriguez [139] arrived at the same concept, but from a very dif-ferent perspective – one inspired by the close mathematical parallels between therecursive structure of multibody system dynamics and those of optimal filteringproblems for time-domain dynamical systems. These parallels are explored furtherin Sect. 6.4.

6.2 The P(k) Articulated Body Inertia

We refer to the sub-system consisting of just links k through 1 as the kth articulatedbody system for the serial-chain. Thus, there are n such articulated body systemsassociated with the system, with the first one being simply link 1 all by itself, andthe nth one being the full serial-chain. We assume for now that the system is at restso that the velocity dependent a(k) Coriolis accelerations and the b(k) gyroscopicforce terms are zero and can be ignored.

We focus first on characterizing the P(k) articulated body inertia term in (6.5).In keeping with the assumptions of the articulated body reference model, all thegeneralized forces in the system are assumed to be zero, i.e., T(k) = 0 for 1�k�n.In Sect. 6.3 we will remove this restriction when we shift attention to the z(k) cor-rection spatial force term in (6.6).

6.2.1 Induction Argument for P(k)

We adopt an induction based argument to show that the articulated body inertia atthe kth link, denoted P(k), is a well-defined, symmetric and positive semi-definitequantity satisfying the force/acceleration relationship in (6.5) for the kth articulatedbody system. We will show that the assumption that the outboard hinge forces arezero imposes a strict relationship between the spatial accelerations of neighboringbodies and their hinge generalized accelerations. This will allow us to develop anexpression for the articulated body inertia.

To start the induction based argument, let us consider the first articulated bodysystem which consists of just link 1 alone. The relationship between a spatial forcef(1) being applied at frame O1 on link 1 and its spatial acceleration α(1) is simplygiven by

f(1) =M(1)α(1)

Thus, choosing the articulated body inertia P(1) as

P(1) =M(1)

satisfies (6.5) for link 1. The symmetry and positive semi-definiteness of P(1) fol-lows from the symmetry and positive semi-definiteness of theM(1) spatial inertia.

6.2 The P(k) Articulated Body Inertia 101

Obtaining the expression for P(1) allows us to begin our induction argument.For the purposes of induction, assume that (6.5) holds for the kth articulated bodysystem, and that its articulated body inertia P(k) is well-defined, symmetric andpositive semi-definite, i.e.,

f(k) = P(k)α(k) with T(j) =H(j)f(j) = 0 ∀ j < k (6.7)

Our goal now is to establish (6.5) for the (k+1)th link, i.e., to derive an expressionfor P(k+ 1) such that

f(k+ 1) = P(k+ 1)α(k+ 1) with T(j) =H(j)f(j) = 0 ∀ j < k+ 1 (6.8)

6.2.2 The D(k) and G(k) Matrices

Define α+(k) asα+

(k) = φ∗(k+ 1,k)α(k+ 1) (6.9)

With the system at rest,α+(k) represents the spatial acceleration of the Ok+1 frame.Thus, the spatial acceleration relationship in the equations of motion in (5.21) canbe restated as

α(k) = α+(k)+H∗

(k)θ(k) (6.10)

a(k) is absent from (6.10) because we have assumed that the system is at rest. Itfollows from (6.7) that

0 6.7= T(k)

6.7= H(k)f(k) =H(k)P(k)α(k)

6.10= H(k)P(k)α+

(k)+H(k)P(k)H∗(k)θ(k) (6.11)

Therefore, the hinge acceleration θ(k) is given by the expression

θ(k)6.11= −D−1

(k)H(k)P(k)α+(k) = −G∗

(k)α+(k) (6.12)

where the quantities G(k) ∈ R6×rv(k) and D(k) ∈ Rrv(k)×rv(k) are defined as

D(k)�= H(k)P(k)H∗

(k) and G(k)�= P(k)H∗

(k)D−1(k) (6.13)

The invertibility of D(k) would necessarily follow from (6.13) if P(k) were non-singular since the hinge map matrix H∗(k) is full-rank. This is true in all but patho-logical cases. We will continue by assuming that D(k) is invertible. Equation (6.12)provides a well defined relationship between the spatial acceleration of frame O

+k

and the generalized acceleration of the kth hinge. G(k) is referred to as the Kalmangain at the kth hinge, and D(k) as the articulated body hinge inertia at the kthhinge.


Remark 6.1 G(k) is a generalized-inverse of H(k).It is easy to verify from the definitions in (6.13) that the following identity holds:

H(k)G(k) = I (6.14)

It follows therefore, that H(k)G(k)H(k) =H(k) and, hence, G(k) is a generalized-inverse1 of H(k).

6.2.3 The τ(k) and τ(k) Projection Matrices

Using (6.12) in (6.10) leads to

α(k)6.10,6.12

= [I−H∗(k)G∗

(k)]α+(k) = τ∗(k)α+

(k) (6.15)

where the quantities τ(k) and τ(k) in R6×6 are defined as

τ(k)�= G(k)H(k) and τ(k)

�= I−τ(k) = I−G(k)H(k) (6.16)

Taken together, (6.12) and (6.15) show that the generalized acceleration of the kthhinge, and the spatial acceleration of the kth link, can be determined directly fromthe α+(k) spatial acceleration of the inboard O

+k frame. τ(k) and τ(k) have the

important property that they are projection matrices.2 This is the subject of thefollowing exercise.

Exercise 6.1 Properties of the τ(k) and τ(k) projection matrices.

1. Show that τ(k), τ∗(k), τ(k) and τ∗(k) are all projection matrices.2. Show that

τ(k)G(k) = G(k) and τ∗(k)H∗(k) =H∗

(k) (6.17)

Thus the range spaces of the τ(k) and τ∗(k) projection operators are in factthe range spaces of G(k) and H∗(k), respectively. Also, it follows that the rangespaces of G(k) and H∗(k) are the null-spaces of the τ(k) and τ∗(k) projections,respectively, that is

τ(k)G(k) = 0 and τ∗(k)H∗(k) = 0 (6.18)

From (6.15), τ∗(k) has the physical interpretation of projecting out the part ofα+(k) that is along the H∗(k) hinge axis in such a way that no generalized force isgenerated at the hinge. This is illustrated in Fig. 6.2.

1 A matrix X is said to be the generalized-inverse of another matrix Y if YXY = Y.2 A matrix U is a projection matrix if and only if it is square and U2 = U.


Remark 6.2 Nulling out of hinge accelerations.It follows from (6.15) and (6.18) that if α+(k) = H∗(k)θ(k), then α(k) = 0! Ineffect, in the articulated body model, accelerations along the hinge axis are nottransmitted across the freely articulated hinge. The only motion that is transmitted

O+k Ok


α(k) = α+⊥(k)

α+(k)

α+⊥(k) = τ∗(k)α+(k)

τ∗(k)α+(k)

Fig. 6.2 Accelerations do not couple along the hinge axis in the articulated bodymodel

is along the other axes where there is rigid coupling. The information about thisselective coupling across the hinge is contained in the τ(k) projection operator.

Remark 6.3 Alternative derivation of τ(k) expression.An alternative reasoning leading to the derivation of the projection operator τ(k)is described here. First we restate the following relationships established earlier:

α(k)5.21,6.9

= α+(k)+H∗

(k)θ(k) and H(k)P(k)α(k)6.7= 0 (6.19)

The unknown quantity in (6.19) is the hinge acceleration θ(k) for the kth hinge. Ifthe kth hinge were frozen, θ(k) = 0 and α(k) = α+(k). However, due to the pres-ence of a hinge, only a part of α+(k) gets transmitted across the kth hinge as α(k).The above equations require that α(k)∈Null[H(k)P(k)]. Hence,α(k) is the (non-orthogonal) projection of α+(k) ontoNull[H(k)P(k)] along Range[H∗(k)].3 Thespecification of the left and right null-spaces of a projection operator uniquely de-fines it. That is, ifQ is a projection operator whose left null-space is specified by the

3 x is said to be a projection of z onto the subspace X along the subspace Y if we can uniquelywrite the decomposition z = x+y, where x ∈X and y ∈ Y. The projection is orthogonal if andonly if the subspaces X and Y are orthogonal to each other.


columns of X, i.e., X∗Q= 0, and whose right null-space is specified by the columnsof Y, i.e.,QY = 0, thenQ is uniquely defined by

Q= I−Y(X∗Y)−1X∗ (6.20)

From the above discussion, the right and left null-spaces of τ∗(k) are defined by:

τ∗(k) :

{τ∗(k)H∗(k) = 0

H(k)P(k)τ∗(k) = 0(6.21)

Using (6.20), the two conditions on the right in (6.21) are thus sufficient to uniquelydefine the τ∗(k) projection expression as the one defined in (6.16).

Exercise 6.2 The G(k) Kalman gain and the link spatial acceleration.For the articulated body model, show that

G∗(k)α(k) = 0 (6.22)

6.2.4 The P+(k) Matrix

Turning to the force relationships in the articulated body model, we have

f(k)6.7= P(k)α(k)

6.15= P(k)τ∗(k)α+

(k) = P+(k)α+

(k) (6.23)

where the matrix P+(k) ∈R6×6 is defined as

P+(k)

�= P(k)τ∗(k) (6.24)

The expression f(k) = P+(k)α+(k) in (6.23), parallels the earlier f(k) = P(k)α(k)

expression. While P(k) represents the effective articulated body inertia of the ktharticulated body system at the Ok frame, P+(k) can be interpreted as the effectivearticulated body inertia of the same system at the O

+k frame, i.e., on the inboard side

of the kth hinge. As a result of crossing the hinge, the inertia P+(k) is no longernon-singular, as shown in the following exercise.

Exercise 6.3 Properties of P+(k).This exercise establishes that P+(k) is a symmetric, positive semi-definite matrix.

1. Show that

P+(k) = τ(k)P(k) = P(k)τ∗(k) = τ(k)P(k)τ∗(k) (6.25)


Since P(k) is symmetric and positive semi-definite, it follows from the lastexpression that P+(k) is symmetric and positive semi-definite as well.

2. Show thatH(k)P+

(k) = 0 and P+(k)H∗

(k) = 0 (6.26)

Thus, P+(k) is singular, with null-space the same as the null-space of τ∗(k).

6.2.5 Conclusion of the Induction Argument for P(k)

The equations of motion at frame Ok+1 on the (k+ 1)th link are given by theexpression

f(k+ 1)5.21= φ(k+ 1,k)f(k)+M(k+ 1)α(k+ 1)

6.23= φ(k+ 1,k)P+

(k)α+(k)+M(k+ 1)α(k+ 1)

6.9=

[

φ(k+ 1,k)P+(k)φ∗

(k+ 1,k)+M(k+ 1)]

α(k+ 1) (6.27)

Comparing this expression with (6.8), we see that choosing P(k+ 1) as

P(k+ 1)�= φ(k+ 1,k)P+

(k)φ∗(k+ 1,k)+M(k+ 1) (6.28)

satisfies (6.8). This expression for P(k+ 1) effectively concludes the inductionbased argument to establish that the articulated body inertias are well-defined forall links in the articulated body model.

Defining ψ(k+ 1,k) ∈R6×6 as

ψ(k+ 1,k)�= φ(k+ 1,k)τ(k) (6.29)

Equation (6.28) can be rewritten in the form

P(k+ 1)6.29= ψ(k+ 1,k)P(k)ψ∗

(k+ 1,k)+M(k+ 1) (6.30)

Equation (6.30) is of the same form as the discrete Riccati equations4 that appearin the optimal filtering of discrete-time systems [5]. The recursion in (6.30) is simi-lar to the recursions for the R(k) composite body inertias in (4.8). From (6.30), it isevident that P(k+ 1) is symmetric and positive definite due to the positive definite-ness of the link spatial inertias. ψ(k+ 1,k) is the force/velocity transformation op-erator for articulated bodies, analogous to the transformation matrix φ(k+ 1,k) forrigid bodies. While the rigid body transformation matrix φ(k+ 1,k) is a function

4 Section 6.4 on page 111 discusses the roots of this terminology in the mathematical parallels withestimation theory.


of l(k+ 1,k) alone, ψ(k+ 1,k) depends additionally on the hinge type and spatialinertias of all the outboard links.


P(k)P(k+1)

P+(k)

P+(k−1)

P(k+1) = φ(k+1,k)P+(k)φ∗(k+1,k)

P+(k) = τ(k)P(k)τ∗(k)

Fig. 6.3 Propagation of the P(k) articulated body inertia computation from the kthto the (k+1)th link

Based on (6.28), an algorithm for the computation of the articulated body inertiasis described in Algorithm 6.1 and illustrated in Fig. 6.3. The recursiveO(N) tip-to-base algorithm propagates the articulated body inertia from body to body and acrosshinges. During this process, other articulated body quantities, such as D(k), G(k),etc., are also computed.

Algorithm 6.1 Recursive algorithm for the articulated body inertias⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(0) = 0

for k = 1 · · ·nP(k) = φ(k,k− 1)P+

(k− 1)φ∗(k,k− 1)+M(k)

D(k) =H(k)P(k)H∗(k)

G(k) = P(k)H∗(k)D−1

(k)

τ(k) = I−G(k)H(k)

P+(k) = τ(k)P(k)

end loop

Remark 6.4 P(k) is not a spatial inertia.The computations for the articulated body inertias are analogous to those thatdetermine the composite spatial inertia matrix in Sect. 4.1.2. However, there is afundamental difference. The composite spatial inertia matrix can be parametrized

6.3 Articulated Body Model Force Decomposition 107

by ten parameters. These parameters corresponded to the mass, mass center, andthe rotational inertia of the composite body outboard of hinge k. However, no suchreduced parametrization of the articulated body inertias is possible. The main rea-son for this is that P(k), while inertia-like, is not a spatial inertia. And thus, theproperty of a parametrization by only ten parameters, and the resultant structure,is lost. The articulated body inertias are fully populated 6× 6 symmetric matricesrequiring a full parametrization by 21 parameters.

Exercise 6.4 Ordering of R(k), P(k) and M(k).Show that

P(k) � P+(k) and R(k) � P(k) �M(k) ∀ k (6.31)

A physical interpretation of the second expression in (6.31) is that, for a com-posite body, the motion of a component body couples fully into the motion of theoutboard bodies, On the other hand, for an articulated body, the motion along thehinge axis is projected out, and, only a part of the motion is coupled into the motionof the outboard bodies. Hence, the P(k) inertia experienced in the articulated bodymodel is smaller than R(k). On the other hand, P(k) is larger thanM(k) because itincludes not only the spatial inertia of the kth body, but the articulated body inertiasof the outboard bodies.

6.3 Articulated Body Model Force Decomposition

If the outboard hinges were truly un-actuated, i.e., there were no hinge forces at theoutboard hinges, then it follows from (6.21) and (6.7) that

α(k) = τ∗α+(k) and f(k) = P(k)α(k) = P+

(k)α+(k)

We now extend the reference articulated body model to handle the general situationwhere the T(·) generalized forces are non-zero. We compensate for the non-zerogeneralized forces by introducing an additional residual force term in (6.6), z(k),such that f(k) = P(k)α(k)+ z(k). This expression will complete the spatial forcedecomposition for the articulated body model.

Once again, we use an induction based argument to establish the force decom-position in (6.6). For the first link, f(1) = P(1)α(1), and thus (6.6) clearly holds forthis case, with z(1) = 0. Let us assume that (6.6) holds for the kth link, and we wishto establish this relationship at the (k+ 1)th link, i.e., we wish to find expressionsfor z(k+ 1) such that f(k+ 1) = P(k+ 1)α(k+ 1)+ z(k+ 1). We have that


f(k)6.16= τ(k)f(k)+τ(k)f(k)

6.16,6.6= G(k)H(k)f(k)+τ(k)

[

P(k)α(k)+ z(k)]

5.21= G(k)T(k)+τ(k)P(k)α(k)+τ(k)z(k)

6.25,6.10= G(k)T(k)+P+

(k)[

α+(k)+H∗

(k)θ(k)]

+τ(k)z(k)

6.26= G(k)T(k)+P+

(k)α+(k)+τ(k)z(k)

That is,f(k) = P+

(k)α+(k)+ z

+(k) (6.32)

where z+(k) is defined as

z+

(k)�= τ(k)z(k)+G(k)T(k) (6.33)

Equation (6.32) extends (6.6) to the O+k frame on the inboard side of the hinge.

Now, from the equations of motion, it follows that

f(k+ 1)5.21= φ(k+ 1,k)f(k)+M(k+ 1)α(k+ 1)

6.32= φ(k+ 1,k)

[

P+(k)α+

(k)+ z+

(k)]

+M(k+ 1)α(k+ 1)

6.9= φ(k+ 1,k)P+

(k)φ∗(k+ 1,k)α(k+ 1)+φ(k+ 1,k)z+(k)

+M(k+ 1)α(k+ 1)

6.28= P(k+ 1)α(k+ 1)+φ(k+ 1,k)z+(k)

Comparing the above with (6.6) at the (k+ 1)th link, we have

f(k+ 1) = P(k+ 1)α(k+ 1)+ z(k+ 1) (6.34)

where z(k+ 1) is defined as

z(k+ 1)�= φ(k+ 1)z

+(k)

6.33,6.29= ψ(k+ 1,k)z(k)+K(k+ 1,k)T(k) (6.35)

with K(k+ 1,k) ∈R6 defined as

K(k+ 1,k)�= φ(k+ 1,k)G(k) (6.36)

Equations (6.34) and (6.35) establish (6.6) for the (k+1)th articulated body system.This concludes the proof by induction.

Figure 6.4 summarizes the spatial force decomposition relations at the differentframe locations on the bodies. The z(k) residual force is only a function of the hingeforces for the hinges outboard of the kth link, and does not depend upon the hingeforces at the inboard hinges. z(k) is zero when all the outboard hinge forces arezero. Table 6.1 compares the properties of the terminal, composite, and articulatedbody models.

6.3 Articulated Body Model Force Decomposition 109


Ok

Ok+1

O+k

O+k−1

f(k) = P(k)α(k)+z(k)f(k+1) = P(k+1)α(k+1)+z(k+1)

f(k) = P+(k)α+(k)+z+(k)

Fig. 6.4 The articulated body inertia decompositions of the f(.) inter-link interactionspatial force at different locations on the kth and the (k+ 1)th links (assumingzero generalized velocities)

Table 6.1 A comparison of the terminal, composite, and articulated body models

Model Terminal body Composite body Articulated body

Reference model Ignore children bod-ies

Ignore outboardhinge accelerations

Ignore outboardhinge generalizedforces

f(k) force decomposition (6.2) (6.4) (6.6)

Inertia recursion (4.7) (6.30)

Inertia dependence Local link inertia Outboard link iner-tias

Outboard link iner-tias and hinges

Residual force recursion (5.42) (5.45) (6.35)

Residual force dependence All link accelerations Outboard hinge ac-celerations

Outboard hinge gen-eralized forces

6.3.1 The ε(k) Vector

Defining ε(k) as

ε(k)�= T(k)−H(k)z(k) (6.37)


z+

(k)6.33= z(k)+G(k)

(

T(k)−H(k)z(k)) 6.37

= z(k)+G(k)ε(k) (6.38)


Algorithm 6.2 describes a recursive algorithm for computing all the z(k) residualforces. The tip-to-base algorithm propagates the z(k) residual forces from body tobody starting at the tip and proceeding towards the base. Along the way, the relatedε(k) terms are also computed. This recursive equation for z(k) is similar in form

Algorithm 6.2 The residual spatial forces vector z(k)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

z+(0) = 0

for k = 1 · · ·nz(k) = φ(k,k− 1)z

+(k− 1)

ε(k) = T(k)−H(k)z(k)

z+

(k) = z(k)+G(k)ε(k)

end loop

to the y(k) composite rigid body inertia term defined in the Exercise 5.5 inversedynamics algorithm.

6.3.2 Acceleration Relationships

We now explore the implications of the force decomposition for the articulated bodymodel on the spatial accelerations. From (6.6) it follows that

T(k)5.21= H(k)f(k)

6.32= H(k)P(k)α(k)+H(k)z(k) (6.39)

Thus,

ε(k)6.39= H(k)P(k)α(k)

5.21= H(k)P(k)

[

φ∗(k+ 1,k)α(k+ 1)+H∗

(k)θ(k)]

6.13= D(k)θ(k)+H(k)P(k)φ∗

(k+ 1,k)α(k+ 1)

(6.40)Rearranging the above, and defining ν(k) as

ν(k)�= D−1

(k)ε(k) (6.41)

it follows that

θ(k)6.40= D−1

(k){ε(k)−H(k)P(k)φ∗

(k+ 1,k)α(k+ 1)}

6.41,6.13= ν(k)−G∗

(k)φ∗(k+ 1,k)α(k+ 1)

6.36= ν(k)−K∗

(k+ 1,k)α(k+ 1) (6.42)

6.4 Parallels with Estimation Theory 111

Moreover

α(k)5.21= φ∗

(k+ 1,k)α(k+ 1)+H∗(k)θ(k)

6.42= φ∗

(k+ 1,k)α(k+ 1)+H∗(k)

[

ν(k)−K∗(k+ 1,k)α(k+ 1)

]

6.29,6.36= ψ∗

(k+ 1,k)α(k+ 1)+H∗(k)ν(k) (6.43)

Comparing the last expression in (6.43) with the expression for spatial accelerationsin (5.21), we can interpret ψ∗(k+ 1,k) as being the transformation matrix in thearticulated body model, analogous to the transformation matrix φ∗(k+1,k) for thecomposite body model. Similarly, ν(k) can be thought of as the relative hinge accel-eration for the articulated body model, analogous to the relative hinge accelerationθ(k) for the composite body model.

Exercise 6.5 Relationship of ν(k) to α(k).Establish the following relationship:

ν(k) = G∗(k)α(k) (6.44)

Unlike the G∗(k)α(k) = 0 expression in (6.22), (6.44) contains the generallynon-zeroν(k) term. The ν(k) term arises from the presence of non-zero generalizedforces at the outboard hinges. This term is not present in (6.22) because it derivationassumes that the outboard generalized forces are zero.

6.4 Parallels with Estimation Theory

Some of the terminology we have used thus far has its roots in optimal estimationtheory. We take a look at such parallels in this section because the mathematicalconnections run deep. Let us begin by considering a discrete-time, time-varying,linear dynamical system described by the following state-transition equations:

x(k) = φ(k,k− 1)x(k− 1)+w(k)

T(k) = H(k)x(k) (6.45)

In the above equations, we have deliberately borrowed notation from the multibodydynamics context to this time-domain problem, to highlight the mathematical par-allels across the domains. However, the meaning of the symbols are quite differentand are as follows:

• k represents the time variable• x(k) is the system state vector at time k• w(k) is an input white noise process with covarianceM(k)

• φ(k,k− 1) is a time-varying state-transition matrix that propagates the state attime (k− 1) to the state at time k


• T(k) is the output observable process at time k• H(k) is the time-varying output matrix at time k that maps the x(k) system state

into the T(k) output process at time k

6.4.1 Process Covariances

In the optimal estimation literature [5], it is known that the covariance matrix forthe x(k) system state, R(k), satisfies the following equation:

R(k) = φ(k,k− 1)R(k− 1)φ∗(k,k− 1)+M(k) (6.46)

This recursive equation is referred to as a Lyapunov equation in the optimal esti-mation literature. Equation (6.46) is identical in form to the recursive relationshipfor composite body inertias in (4.7) on page 59. The cross-covariances of the sys-tem state at different time instants are given by the elements of the φMφ∗ matrix,where the φ, H, and M matrices have structure identical to the matrix definitions in(3.32), (3.37), and (4.4), respectively. The cross-covariances of the output process,T(k), are the elements of the M = HφMφ∗H∗ matrix, which is identical in formto the mass matrix of the serial-chain system.

6.4.2 Optimal Filtering

The optimal filtering problem consists of generating an optimal estimate of the un-known internal state, x(k), from just the knowledge of the current and past noisyobservations, namely the T(j) values prior to, and including, time k. The optimalsmoothing problem consists of using additional observations after time k to opti-mally improve the filtered state estimate at time k. Thus, the optimal filtering processis said to be causal, since it is only uses current and past observations. On the otherhand, the optimal smoothing process is anti-causal, since it is allowed to use futureobservations to improve the current estimate.

Kalman and others [5,91,93] developed elegant solutions to the optimal filteringand smoothing problems. The optimal filter estimate of the system state at timek, denoted z+(k), satisfies a recursive relationship with the optimal filter estimate,z+(k− 1), from the previous time step k− 1. We sketch out this two stage filterestimate generation technique that takes place at each time step:

1. First, with no additional observations, the optimal filter estimate from the previ-ous time k−1, z+(k− 1), is propagated to form a prediction z(k) of the optimalfilter estimate of x(k) at time k.

2. Next, the new observation T(k) at time k, is processed to remove the part thatis predictable from the past observations, to create a residual innovations term,ε(k), containing just the new information. This innovation term is used to update

6.4 Parallels with Estimation Theory 113

the predicted optimal filter estimate, z(k), to obtain the optimal filter estimate,z+(k), at time k.

This 2-step recursive procedure is applied at each new time step as new observationvalues are obtained.

The covariance of the optimal filter estimate error, x(k)−z(k), at time k, denotedP(k), satisfies the equation

P(k) = ψ(k,k− 1)P(k− 1)ψ∗(k,k− 1)+M(k) (6.47)

This equation is referred to as the discrete form of the Riccati equation. This equa-tion is identical in form to the (6.30) for articulated body inertia quantities. Thedefinition of ψ(k,k− 1) is mathematically identical to that in (6.29).

Given the optimal filtered estimate at time (k−1), z+(k− 1), the best predictionof the system state (without any additional observations), z(k), is obtained by simplypropagating this estimate using the state-transition matrix as follows:

z(k) = φ(k,k− 1)z+

(k− 1) (6.48)

Once the new observation at time k, T(k), is available, it can be used to first extractthe new information in the observation, ε(k), and can then be used to update z(k) toobtain the new optimal filtered estimate at time k, z+(k), as follows:

ε(k) = T(k)−H∗(k)z(k)

z+

(k) = z(k)+G(k)ε(k) (6.49)

D(k) =H(k)P(k)H∗(k) is the covariance of the ε(k) innovations process. G(k) =

P(k)H∗(k)D−1(k) is known as the Kalman gain in the estimation literature, anddepends directly on the P(k) covariance obtained as a solution to the Riccati equa-tion. Observe that the optimal filter estimate at time k only depends on the pastobservations and, hence, this optimal filter is said to be a causal filter.

Equations (6.48) and (6.49) are identical in form to (6.35), (6.37), and (6.38)from the multibody context. Thus, we have observed that several quantities in thearticulated body model are mathematically equivalent to terms in Kalman optimalfiltering for discrete-time systems.

6.4.3 Optimal Smoothing

The parallels continue. The optimal smoothed estimate, of the x(k) state at time k,f(k), is based on the knowledge of the T(j) observations over the full time interval1 through n, and not on just the past observations, as for the z+(k) optimal fil-tered estimate. The smoothed estimate, f(k), can be obtained by updating the filteredestimate as follows:

f(k) = z(k)+P(k)α(k) = z+(k)+P+(k)α+

(k) (6.50)


Theα and α+(k) quantities above are obtained by an anti-causal recursion that usesthe future observations in a time-reversed recursion that is mathematically identicalto (6.43). Equation (6.50) is precisely the form of the force decomposition from thearticulated body model in (6.32) and (6.34)!

6.4.4 Extensions

The mathematical parallels between estimation theory and multibody dynamics areextensive. In the next chapter, we derive a new factorization of the mass matrix andan analytical expression for its inverse. These results mirror spectral factorizationtechniques from estimation theory [20, 92]. These parallels were first identified byRodriguez and discussed in [139–143, 145].

However, it is also appropriate to point out areas where the parallels start tobreakdown and the domains diverge. Unlike the strict sequential order imposed bytime for discrete-time systems, no such ordering is required for the links in multi-body systems. The link numbering is not restricted to the parent/child sequence.Moreover, when we explore non-serial-chain multibody systems with branchedtopologies, the sequential parallels with time are again less obvious. However, aswe will see in later chapters, it is remarkable that, with modest extensions, severalof the insights developed by exploiting the mathematical parallels between time-domain and serial-chain system domains continue to hold for more general multi-body systems.

Chapter 7Mass Matrix Inversion and AB ForwardDynamics

This chapter uses the articulated body model to derive a new Innovations OperatorFactorization of the serial-chain mass matrix, M. This factorization has square fac-tors, in contrast to the non-square Newton–Euler factorization of M =HφMφ∗H∗in (5.23). The Innovations factorization is subsequently used to obtain an explicitanalytical operator expression for the inverse of the mass matrix. These factoriza-tions have many important uses, which will be explored in subsequent chapters.

At this stage, we continue to focus on serial-chain rigid multibody systems. How-ever, we will see later that these operator factorization and inversion results andidentities continue to hold for a very broad class of multibody systems.

7.1 Articulated Body Spatial Operators

First, we summarize the recursive relationships for articulated body quantitiesdefined in Algorithm 6.1 on page 106 in the previous chapter:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(0) = 0, τ(0) = 0

for k = 1 · · ·nψ(k,k− 1) = φ(k,k− 1)τ(k− 1)

P(k) = φ(k,k− 1)P+(k− 1)φ∗(k,k− 1)+M(k)


G(k) = P(k)H∗(k)D−1(k)

K(k+ 1,k) = φ(k+ 1,k)G(k)


P+(k) = τ(k)P(k)

end loop

(7.1)


116 7 Mass Matrix Inversion and AB Forward Dynamics

ψ(k+ 1,k) is the force/velocity transformation matrix for the articulated bodymodel analogous to the φ(k+ 1,k) transformation matrix for the composite bodymodel. While φ(k+ 1,k) is a function of kinematical quantities alone, ψ(k+ 1,k)depends additionally on the hinge types and spatial inertias of all the outboard links.Also, unlike φ(k+ 1,k), ψ(k+ 1,k) is not invertible.

Now define the block diagonal operator P as

P�= diag

{P(k)

}nk=1

∈R6n×6n (7.2)

and the following additional spatial operators:

D�= diag

{D(k)

}nk=1

=HPH∗ ∈RN×N

G�= diag

{G(k)

}nk=1

= PH∗D−1 ∈R6n×N

K�= EφG ∈R6n×N

τ�= diag

{τ(k)

}nk=1

= GH ∈R6n×6n

τ�= diag

{τ(k)

}nk=1

= I−τ ∈R6n×6n

P+ �= diag

{P+(k)

}nk=1

= τPτ∗ = τP = Pτ∗ ∈R6n×6n

Eψ�= Eφτ ∈R6n×6n

(7.3)

The operators D, G, τ, τ, and P+ are all block diagonal, with the kth diagonal entrybeing defined by the recursions in (7.1). On the other hand, K has non-zero elementsonly along its first sub-diagonal and has the form:

K =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0

K(2,1) 0 . . . 0 0

0 K(3,2) . . . 0 0...

.... . .

......

0 0 . . . K(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(7.4)

Eψ has the following similar structure:

Eψ =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0ψ(2,1) 0 . . . 0 0

0 ψ(3,2) . . . 0 0...

.... . .

......

0 0 . . . ψ(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(7.5)

7.1 Articulated Body Spatial Operators 117

The structure of Eψ is the same as that of Eφ and, like it, Eψ is nilpotent (Enψ = 0).

Analogous to φ in (3.36), we use Lemma A.1 on page 400 to defineψ ∈R6n×6n

ψ�= (I−Eψ)−1 = I+Eψ+E2

ψ+ · · · +En−1ψ

=

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0

ψ(2,1) I . . . 0...

.... . .

...

ψ(n,1) ψ(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

(7.6)

whereψ(i, j)

�= ψ(i, i− 1) · · · ψ(j+ 1, j) for i > j

The structure of ψ is very similar to that of φ, and its elements have semi-groupproperties that can be used in the development of recursive relationships and al-gorithms using the lemmas from Sect. 3.5. φ may be viewed as the transformationoperator for the composite body model (i.e., as if all the hinges are locked), andψ asthe transformation operator for the articulated body model (i.e., as if all the hingesare free).

Table 7.1 summarizes the differences and similarities between the compositerigid body model and the articulated body model for serial-chains using the abovearticulated body inertia operators and component level expressions from Chap. 6.

7.1.1 Some Operator Identities

Lemma 7.1 Riccati equation for articulated body inertias.P satisfies the following Riccati equation

M = P−EψPE∗ψ = P−EψPE∗

φ (7.7)

Proof: The first equality in (7.7) is merely a restatement in stacked notation ofthe following recursive component-wise definition of the P(k) matrices from (6.30)

P(k+ 1)6.29= ψ(k+ 1,k)P(k)ψ∗(k+ 1,k)+M(k+ 1)

Moreover,

EψPE∗ψ

7.3= EψPτ∗E∗

φ7.3= EψτPE∗

φ7.3= Eφτ τPE∗

φ = EψPE∗φ

Using this in the first equality in (7.7) leads to the second equality.


Tab

le7.

1C

ompa

riso

nbe

twee

nco

mpo

site

and

arti

cula

ted

body

mod

els,

whi

leig

nori

ngth

eC

orio

lis

and

gyro

scop

icte

rms

Var

iabl

eC

ompo

site

body

Art

icul

ated

body

Rel

ativ

ehi

nge

acce

lera

tion

θν

αφ

∗ H∗ θ

ψ∗ H

∗ ν

α(k

)φ

∗ (k

+1,k

)α(k

+1)

+H

∗ (k

)θ(k

)ψ

∗ (k

+1,k

)α(k

+1)

+H

∗ (k

)ν(k

)

αw

ith

resp

ectt

oα

+α

++H

∗ θτ∗ α

++H

∗ ν

(τ∗ α

=τ∗ α

+)

νG

∗ α+

+θ

G∗ α

Eff

ectiv

ein

erti

aR

P

Rel

atio

nshi

pto

MR

−Eφ

RE

∗ φP

−Eψ

PE

∗ ψ

Iner

tia

recu

rsio

nsR

+(k

)=

R(k

)P

+(k

+1)

=τ(k

)P(k

)τ∗ (k

)

R(k

+1)

=φ

(k+

1,k

)R+

(k)φ

∗ (k

+1,k

)+M

(k)

P(k

+1)

=φ

(k+

1,k

)P+

(k)φ

∗ (k

+1,k

)+M

(k)

fon

(–si

de)

Rα

+y

Pα

+z

Cor

rect

ion

forc

e(–

side

)y

=φ

RH

∗ θz

=φ

PH

∗ ν

fon

+si

deR

+α

++y

+P

+α

++

z+

Cor

rect

ion

forc

e(+

side

)y

+=y

+RH

∗ θz+

=z+

PH

∗ ν


Extensions of the Riccati equation in Lemma 7.1, and the operator identitiesin Lemma 7.2 for general multibody systems, are discussed in Sect. 9.4. Now, weestablish several operator identities that will be needed later in this chapter.

Lemma 7.2 Useful spatial operator identities.

1.ψ

�= ψEψ = Eψψ=ψ− I (7.8)

2.ψMψ∗ = P+ ψP+Pψ∗ (7.9)

3.ψ−1 −φ−1 = KH (7.10)

4.ψ−1φ= I+KHφ

φψ−1 = I+φKH

φ−1ψ= I−KHψ

ψφ−1 = I−ψKH

(7.11)

5.[I−HψK]Hφ=Hψ

φK[I−HψK] =ψK

[I+HφK]Hψ=Hφ

ψK[I+HφK] = φK

(7.12)

6.HψMψ∗H∗ = D (7.13)

Proof:

1. The proof is identical to that of (3.41) in Exercise 3.6 except that we now workwith ψ and Eψ instead of φ and Eφ.

2. Pre- and post-multiplying (7.7) by ψ and ψ∗ we have

ψMψ∗ =ψPψ∗ −ψEψPE∗ψψ

∗ 7.8= (ψ+ I)P(ψ+ I)∗ − ψPψ∗

= P+ ψP+Pψ∗

3. From (7.6),

ψ−1 = I−Eψ7.3= I−Eφτ

7.3= (I−Eφ)+Eφτ

3.36,7.3= φ−1 +EφGH

7.3= φ−1 +KH

4. Pre- and post-multiplying (7.10) by φ leads to the first pair of identities. Repeat-ing the process, using ψ, leads to the latter pair.


5. We have

[I−HψK]Hφ=H[I−ψKH]φ7.11= H(ψφ−1)φ=Hψ

Similar use of the other identities in (7.11) leads to the remaining ones in (7.12).6. We have

HψMψ∗H∗ 7.7= H(P+ ψP+Pψ∗)H∗ 7.3

= D+HψPH∗ +HPψ∗H∗

7.8= D+HψEψPH∗ +HPE∗

ψψ∗H∗

7.3= D+HψEφτPH

∗ +HPτ∗E∗φψ

∗H∗

7.3= D+HψEφP+H∗ +HP+E∗

φψ∗H∗

6.26= D

7.1.2 Innovations Operator Factorization of the Mass Matrix

The following lemma establishes a new operator factorization, referred to as theInnovations Operator Factorization of the of the serial-chain mass matrix, M.

Lemma 7.3 Innovations operator factorization of the mass matrix.The mass matrix, M, has the following square factorization, referred to as the Inno-vations Operator Factorization:

M =[

I+HφK]

D[

I+HφK]∗

(7.14)

Proof: We have that

M5.25= HφMφ∗H∗ =H(φψ−1)ψMψ∗(φψ−1)∗H∗

7.11= H[I+φKH]ψMψ∗[I+φKH]∗H∗

= [I+HφK](HψMψ∗H∗)[I+HφK]∗ 7.13= [I+HφK]D[I+HφK]∗

The factor [I+HφK] ∈ RN×N is square and block lower-triangular, while D

is block diagonal. Thus, the factorization in (7.14) may be thought of as an analyt-ical block LDL∗ factorization of the mass matrix. This factorization is known asthe Innovations Operator Factorization because of its relationship to factorizationresults in the innovations approach to filtering and prediction theory discussed in


Sect. 6.4 on page 111. In contrast to the Innovations Factorization, the factors in theNewton–Euler Operator Factorization of the mass matrix (5.23) are not square.

In (4.10) on page 61, we encountered the following decomposition of φMφ∗using the composite body inertias R

φMφ∗ = R+ φR+Rφ∗

and in (7.9), the following decomposition of ψMψ∗ using P

ψMψ∗ = P+ ψP+Pψ∗

Exercise 7.1 develops decompositions of φMφ∗ and φMψ∗ using P.

Exercise 7.1 Decomposition of φMφ∗ using P.

1. Derive the following disjoint decomposition ofφMψ∗ using the articulated bodyinertias:

φMψ∗ = P+ φP+Pψ∗ (7.15)

2. Also, derive the following decomposition of φMφ∗:

φMφ∗ = P+ φP+Pφ∗ +φKDK∗φ∗ (7.16)

3. Use (7.16) to show that R � P.

7.1.3 Operator Inversion of the Mass Matrix

The following lemma derives an analytical expression for the inverse of [I+HφK].

Lemma 7.4 The operator inverse of [I+HφK].[I+HφK] has an operator inverse given by

[I+HφK]−1 = [I−HψK] (7.17)

Proof: From the standard matrix identity (I+AB)−1 = I−A(I+BA)−1B from(A.18) on page 400, we have (with A =H and B= φK):

[I+HφK]−1 = I−H[I+φKH]−1φK7.11= I−H(φψ−1)−1φK

= I−HψK


Since [I+HφK] is lower-triangular, it follows that so is its inverse [I−HψK].By combining Lemmas 7.3 and 7.4, we obtain the following operator expression forthe inverse of the mass matrix.

Lemma 7.5 Operator factorization of M−1.The inverse of the mass matrix is given by the expression:

M−1 = [I−HψK]∗D−1[I−HψK] (7.18)

Proof: We have

M−1 7.14= {[I+HφK]D[I+HφK]∗}−1 = [I+HφK]−∗D−1[I+HφK]−1

7.4= [I−HψK]∗D−1[I−HψK]−1

This result is remarkable in that it provides an explicit, analytical expression forM−1. The factor [I −HψK] is square and block lower-triangular, while D−1 isblock diagonal. Thus, the factorization in (7.17) may be thought of as an analyticalblock L∗DL factorization of the mass matrix inverse. The following section appliesthese analytical operator expressions to develop an efficient solution to the forwarddynamics problem for serial-chain systems. The mass matrix operator factorizationand inversion expressions described in this section are based on [71, 140, 149, 151].

7.2 Forward Dynamics

In the forward dynamics problem, the known quantities are the T hinge forces andthe (θ, θ) current state, and the quantities to be computed are the θ hinge acceler-ations. Thus, this problem requires solving for the θ accelerations vector from thefollowing equations of motion:

M(θ)θ+C(θ, θ) = T (7.19)

given the state (θ, θ) and the vector of generalized forces, T. This is only a concep-tual statement of the problem, since, typically, neither the mass matrix M nor theCoriolis forces vector C are available. We describe below possible approaches forsolving this problem [71]:

1. The direct approach is to compute M(θ) using the O(N2) composite body iner-tia algorithm, and the Coriolis vector C(θ, θ) using the O(N) inverse dynamicsprocedure, and to subsequently solve the linear matrix equation in (7.19). Sincesolving an N-size linear matrix equation is an O(N3) procedure, this leads toa forward dynamics algorithm of overall O(N3) cost. The cubic computationalcost makes this algorithm very expensive for even moderately sized systems.

7.2 Forward Dynamics 123

2. An alternative is to take advantage of the Innovations factorization of the massmatrix

M = [I+HφK]D[I+HφK]∗

in (7.14) to develop an O(N2) forward dynamics algorithm. This is possible be-cause, while solving a general N×N linear matrix equation is an O(N3) proce-dure, it is of only O(N2) complexity when the coefficient matrix happens to betriangular. Once again, assuming that C has already been computed and is avail-able, theO(N2) forward dynamics procedure involves the successive solution oftwo triangular systems of equations outlined in the following steps [155]:

a. Compute the [I+HφK] matrix.b. Solve [I+HφK]x= T −C for x.c. Solve [I+HφK]∗ θ= D−1x for θ.

In contrast with the O(N3) methods discussed above, this method requires theexplicit computation of the articulated body inertia quantities and the [I+HφK]factor, but does not require the explicit computation of the mass matrix itself.Thus, the development of the Innovations Operator Factorization has paid off inreducing the computational cost of the forward dynamics problem from O(N3)to O(N2) computational cost.

3. The following section describes an even lower order algorithm for solving theforward dynamics problem, that takes advantage of the analytical expression forthe mass matrix inverse in Lemma 7.5.

7.2.1 O(N) AB Forward Dynamics Algorithm

We now develop an optimalO(N) forward dynamics algorithm based on the expres-sion for the mass matrix inverse derived in Lemma 7.5. Using this lemma in (7.19)leads to the following operator expression for θ:

θ = M−1(T −C) = [I−HψK]∗D−1[I−HψK](T −C) (7.20)

The following lemma derives a simpler form of (7.20).

Lemma 7.6 Operator expression for the generalized accelerations θ.The operator expression for θ is given by

θ= [I−HψK]∗D−1[T −Hψ(KT +Pa+b)]

−K∗ψ∗a (7.21)

Proof: We have

θ7.20= [I−HψK]∗D−1[I−HψK](T −C)

5.25= [I−HψK]∗D−1[I−HψK] {T −Hφ(Mφ∗a+b)}

= [I−HψK]∗D−1[I−HψK]T

−[I−HψK]∗D−1[I−HψK]Hφ(Mφ∗a+b)

(7.22)


Now,

[I−HψK]Hφ(Mφ∗a+b)

7.12= Hψ(Mφ∗a+b)

7.15= H

({ψP+Pφ∗}a+ψb

)

7.3= (HψP+DK∗φ∗)a+Hψb =Hψ(Pa+b)+DK∗φ∗a

(7.23)

Using this in the second half of (7.22), it follows that

[I−HψK]∗D−1[I−HψK]Hφ(Mφ∗a+b)

7.23= [I−HψK]∗D−1 [Hψ(Pa+b)+DK∗φ∗a]

7.11= [I−HψK]∗D−1Hψ(Pa+b)+K∗ψ∗a

(7.24)

Using this in (7.22) leads to (7.21).Now group together sub-expressions in (7.21) to define intermediate quantities

as shown below:

θ = [I−HψK]∗D−1[T −Hψ(KT +Pa+b)︸︷︷︸z︸︷︷︸

ε

]

︸︷︷︸ν

−K∗ψ∗a

Using these intermediate quantities, (7.21) can be re-expressed as

z�= ψ(KT +Pa+b) (7.25a)

ε�= T −Hz

7.25a= T −Hψ(KT +Pa+b) (7.25b)

ν�= D−1ε

7.25b= D−1[T −Hψ(KT +Pa+b)] (7.25c)

θ7.21,7.25c

= [I−HψK]∗ν−K∗ψ∗a (7.25d)

The following exercise introduces the new z+ term to further simplify the expressionfor z.

Exercise 7.2 Expression relating z and z+.

1. Derive the following operator expressions for z that are an alternative to (7.25a):

z = φ[

Kε+Pa+b]

(7.26)

2. With z+ defined as

z+�= z+Gε (7.27)

show thatz = Eφz+ +Pa+b (7.28)


3. Show that

z+�= φ

[

Gε+Pa+b]

(7.29)

Exercise 7.3 Expression for α in terms of ν.We have from (5.23) that α=φ∗(H∗ θ+a). Derive the following alternative expres-sion for α:

α=ψ(H∗ν+a) (7.30)

Furthermore, show that the generalized and innovations accelerations are relatedto each other by the following expressions:

θ = ν−K∗α= ν−G∗α+ where α+ �= col

{α+(k)

}nk=1

6.9= E∗

φα (7.31)

Observe that the (ψ∗,ν) pair play a very similar role to (φ∗, θ) in the expressionfor the α spatial accelerations. We can think of the former pair as corresponding tothe articulated body inertia model while the latter pair corresponds to the compositebody model. Due to these parallels, we refer to ν as the innovations accelerationsfor the system.

The operator expressions in (7.25) can be simplified using Exercises 7.2 and 7.3as follows:

z7.28= Eφz+ +Pa+b (7.32a)

z+7.27= z+Gε (7.32b)

ε7.25b= T −Hz (7.32c)

ν7.25c= D−1ε (7.32d)

α+ = E∗φα (7.32e)

θ7.31= ν−G∗α+ (7.32f)

α = α+ +H∗θ+a (7.32g)

These expressions are essentially stacked vector representations of the correspond-ing component link terms (e.g. z(k), ε(k)) introduced in the discussion of the ar-ticulated body model in Chap. 6. The equivalence is complete except that the aboveexpressions are more general; they allow non-zero velocities and include the a andb terms.

The operator expressions in (7.32) can be converted into recursive computationalalgorithms without requiring the explicit computation of the component operatorsusing lemmas from Sect. 3.5 on page 51. The resulting O(N)articulated body (AB)forward dynamics procedure is described in Algorithm 7.1 [47, 139].


Algorithm 7.1 The O(N) AB forward dynamics algorithm

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(0) = 0, z+(0) = 0, T(0) = 0, τ(0) = 0

for k = 1 · · ·nP(k) = φ(k,k− 1)P+(k− 1)φ∗(k,k− 1)+M(k)


G(k) = P(k)H∗(k)D−1(k)


P+(k) = τ(k)P(k)

z(k) = φ(k,k− 1)z+(k− 1)+P(k)a(k)+b(k)


ν(k) = D−1(k)ε(k)

z+(k) = z(k)+G(k)ε(k)

end loop

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

α(n+ 1) = 0

for k = n · · ·1α+(k) = φ∗(k+ 1,k)α(k+ 1)

θ(k) = ν(k)−G∗(k)α+(k)

α(k) = α+(k)+H∗(k)θ(k)+a(k)

end loop

The first tip-to-base recursive sweep contains steps for computing the P(.)s andz(.)s articulated body inertia quantities discussed in Chap. 6. One difference hereis in the computation of the z(.) terms, since we no longer assume that the systemis at rest. These steps now include contributions from the a(k) Coriolis accelera-tion and b(k) gyroscopic force terms. The second sweep is a base-to-tip recursionthat uses the ν(k), and other terms computed in the first sweep, to compute thehinge and spatial accelerations for all the links. Referring back to the parallels withtime-domain, optimal filtering discussed in Sect. 6.4, the steps in the tip-to-base re-cursion are analogous to the steps in an optimal filter recursion for computing stateestimates. Similarly, the steps in the base-to-tip Step 4 recursion are analogous tothe steps in an optimal smoother recursion for computing smoothed state estimates.

The Steps 1 and 2 sweeps shown in the illustration of the forward dynamicsalgorithm in Fig. 7.1 compute the kinematic velocity dependent Coriolis and gyro-scopic terms for all the bodies in base-to-tip recursions. This pair of sweeps can becombined into a single base-to-tip sweep. The AB algorithm recursions are shownas Steps 3 and 4 in the figure.


Step 1 Step 2 Step 3 Step 4

Base to tipkinematicssweep

Base to tipvelocity andCoriolisaccelssweep

Tip to basearticulatedbodysweep

Base to tipaccelerationssweep

Fig. 7.1 Structure of theO(N) AB Algorithm 7.1 for serial-chain forward dynamics

The AB algorithm has O(N) computational cost because the cost of each of thesteps in the above algorithm is fixed, and the steps are carried outn times during thecourse of the algorithm. The AB algorithm the fastest forward dynamics algorithmfor systems with greater than 6–8 bodies. A detailed discussion of the computationalcosts and issues for the AB algorithm can be found in Featherstone [51]. In addition,the AB algorithm is also numerically more robust compared with alternatives [13],a well known feature of Kalman filtering techniques as well.

This forward dynamics algorithm does not require the explicit computation ofeither M or C. Indeed, it does not require the explicit computation of any of thespatial operators. This is another illustration of the ease with which operator-levelmanipulations can be used to establish key identities and results. At a later stage,when the time for computations arises, these results can be mapped into highlyefficient computational algorithms.

The general version of the AB algorithm was originally derived from first prin-ciples in Featherstone [47]. Rodriguez [139] derived the same algorithm using spa-tial operator methods exploiting the mathematical parallels from optimal estimationtheory. Several other notable variations of the AB forward dynamics algorithm havebeen published in the literature [6, 10, 16, 28, 154, 156, 178]. A comparison of sev-eral of these techniques is described in Jain [71]. Other approaches to solving theforward dynamics algorithm are described in [23, 56, 89, 113, 115].

Exercise 7.4 Relationship between ν, α and a.Establish the following relationships:

α= τ∗α+ +H∗ν+a

ν = G∗[α−a](7.33)

Observe that the latter expression is similar to the one in Exercise 6.5 on page 111.


An interesting application of the mass matrix operator factorization and inversiontechnique, together with the AB algorithm, to the problem of computing the inverseof manipulator Jacobian matrix, J is discussed in Rodriguez and Scheid [150].

7.3 Extensions to the Forward Dynamics Algorithm

Having developed the basic AB forward dynamics algorithm, we now look at somerelated issues and extensions to the algorithm.

7.3.1 Computing Inter-Link f Spatial Forces

The AB forward dynamics algorithm described in the previous section does notexplicitly compute the f(k) body interaction spatial forces. These forces may berequired in certain situations. One option is to use the body spatial accelerationcomputed in the forward dynamics algorithm to run the second tip-to-base recursionin the Newton–Euler inverse dynamics algorithm (Algorithm 5.2) to compute thesespatial forces. The following exercise describes a better alternative based on thearticulated body model that avoids the additional recursion and allows us to directlycompute f(k) for any link, as needed.

Exercise 7.5 Computing inter-link spatial force f(k).Show that the inter-link spatial forces f are given by:

f = P(α−a)+z = P+α+ +z+ (7.34)

These expressions are generalizations of the force decompositions discussed inSect. 6.3 for the articulated body model that assumed zero generalized velocities.They provide a direct way to compute the inter-link spatial forces, without the needfor additional recursions, once α and z from the forward dynamics solution areavailable.

7.3.2 Including Gravitational Accelerations

Exercise 5.4 on page 87 shows that a uniform gravitational field represented by a

g =

[

0

gl

]

linear gravitational spatial acceleration vector can be included in the equa-

tions of motion as a pseudo spatial acceleration at the base-body. The more generalexpression for C(θ, θ) that includes the gravity term (see (5.39) on page 87) is:

7.3 Extensions to the Forward Dynamics Algorithm 129

C(θ, θ) =Hφ[

Mφ∗(a+E∗g)+b]

(7.35)

with E defined by (4.14). The following exercise derives the generalizations of theAB forward dynamics algorithm to accommodate gravitational accelerations.

Exercise 7.6 Including gravitational accelerations.Assume that the system is under the influence of a uniform, linear gravitationalacceleration field g.

1. Show that the updated expression for (7.21) with gravity included is as follows:

θ= [I−HψK]∗{D−1 [T −Hψ(KT +Pa+b)]−G∗E∗g

}−K∗ψ∗a (7.36)

where E is defined by (5.40).2. Define ν as

ν�= ν−G∗E∗g (7.37)

Show that the altered forms of (7.30) and (7.31) are as follows:

α = ψ∗ (H∗ν+a) (7.38a)

θ = [I−HψK]∗ ν−K∗ψ∗a (7.38b)

= ν−G∗α+ (7.38c)

3. Derive the following new version of (7.33):

α = τ∗α+ +H∗ν+a

ν = G∗[α−a] (7.39)

4. Derive the following new version of the force decomposition relationships in(7.34):

f = P(

α+ E∗g−a)

+z = P+(

α+ + E∗g)

+z+ (7.40)

5. Show that the forward dynamics step for θ(k) in Algorithm 7.1 needs to be mod-ified as follows to handle the gravitational acceleration:

θ(k) = ν(k)−G∗(k)α+(k) (7.41)

7.3.3 Including External Forces

External forces at nodes on the bodies can be incorporated directly into the forwarddynamics algorithm as well. Using (5.32), one approach is to absorb the externalforces on the kth link into the b(k) term. Thus, the external forces can be incorpo-rated into the z(k) expression in Algorithm 7.1 by altering it as follows:


z(k) = φ(k,k− 1)z+(k− 1)+P(k)a(k)+b(k)

−∑i

φ(Bk,Oik)fiext(k)

(7.42)

The general version of Algorithm 7.1, extended to handle gravity and externalforces, is shown in Algorithm 7.2.

Algorithm 7.2 The generalizedO(N) AB forward dynamics algorithm

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(0) = 0, z+(0) = 0, T(0) = 0, τ(0) = 0

for k = 1 · · ·nP(k) = φ(k,k− 1)P+(k− 1)φ∗(k,k− 1)+M(k)


G(k) = P(k)H∗(k)D−1(k)


P+(k) = τ(k)P(k)

z(k) = φ(k,k− 1)z+(k− 1)+P(k)a(k)+b(k)−∑i

φ(Bk,Oik)fiext(k)


ν(k) = D−1(k)ε(k)

z+(k) = z(k)+G(k)ε(k)

end loop

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

α(n+ 1) = 0

for k = n · · ·1α+(k) = φ∗(k+ 1,k)α(k+ 1)

ν(k) = ν(k)−G∗(k)g

θ(k) = ν(k)−G∗(k)α+(k)

α(k) = α+(k)+H∗(k)θ(k)+a(k)

end loop

7.3.4 Including Implicit Constraint Forces

For constrained system dynamics (discussed in Chap. 11), the system is subject toconstraint forces at constrained nodes on the bodies. These constraint forces are

7.3 Extensions to the Forward Dynamics Algorithm 131

implicitly defined, and their value is determined based on the “free” generalizedacceleration values, θf, obtained by using the forward dynamics algorithm on theunconstrained system. Once the constraint forces are known, it is necessary to de-termine the “correction” generalized accelerations, θδ, resulting from the constraintforces. The full constrained dynamics solution for the θ generalized accelerations isgiven by

θ = θf+ θδ

withθf = M−1(T −C) and θδ = −M−1J∗fc (7.43)

Here fc denotes the implicit constraint forces at nodes on the bodies. The discussionof the methods to compute fc are deferred until Chap. 11. It is assumed here that ithas been computed and is available. The objective here is to derive a relationship be-tween the constraint forces and the θδ correction generalized accelerations resultingfrom them. Now,

θδ7.43= −(I−HψK)∗D−1(I−HψK)HφBfc

7.12= −(I−HψK)∗D−1HψBfc

(7.44)

Equation (7.44) provides an expression for θδ in terms of the constraint forces.Based on this expression, an O(N) procedure for computing θδ is shown inAlgorithm 7.3. The algorithm begins with a tip-to-base recursion, followed by abase-to-tip recursion, to compute the correction generalized accelerations.

Algorithm 7.3 Correction θδ accelerations for constraint forces

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

zδ(0) = 0

for k = 1 · · ·nzδ(k) = ψ(k,k− 1)zδ(k− 1)−

∑i

φ(Bk,Oik)fic(k)

ν(k) = −D−1(k)H(k)zδ(k)

end loop⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

λ(n+ 1) = 0

for k = n · · ·1λ(k) =ψ∗(k+ 1,k)λ(k+ 1)+H∗(k)ν(k)

θδ(k) = ν(k)−K∗(k+ 1,k)λ(k+ 1)

end loop

Part IIGeneral Multibody Systems

Chapter 8Graph Theory Connections

So far, we have studied the kinematics and dynamics of serial-chain multibodysystems. Key developments have included the introduction of stacked vectors andspatial operators, such as φ, to provide succinct descriptions of the system levelrelationships. The spatial operators led to analytical developments, such as the In-novations Factorization of the mass matrix, and analytical expressions for the massmatrix inverse. These results also formed the basis for efficient computational algo-rithms for the system mass matrix, and for solving the inverse and forward dynamicsproblems.

Associated with multibody systems are directed graphs that describe the topol-ogy of the system and play an important role in formulating their dynamics. In thischapter, we begin exploring concepts from graph theory that allow us to generalizethe spatial operator formulation to the broader class of multibody systems.

8.1 Directed Graphs and Trees

We begin with an overview of terminology and concepts from graph theory. A graphis a collection of nodes, and edges connecting pairs of nodes. A directed graph(also known as a digraph) is a graph where the edges have direction, i.e., an edgefrom one node to another is not the same as an edge in the reverse direction [183].Each edge in a digraph defines a parent/child relationship between the node pair ofthat edge. Nodes defining an edge are said to be adjacent nodes. The node fromwhich the edge emanates is referred to as the parent node, and the destination nodeis said to be the child node. The set of parent nodes of the kth node is denoted ℘(k),and the set of its children nodes is �(k). Nodes without parent nodes are referred toas root nodes. Digraphs can have zero, one, or multiple root nodes. We assume thatthere is at most a single edge in the same direction between any pair of nodes, i.e.,parallel edges between a pair of nodes are not allowed.

A node, j, is said to be the ancestor of another node, i, if there is a directed pathfrom the node j to the node i. We use the notation i ≺ j (or equivalently j � i) to


136 8 Graph Theory Connections

indicate that node j is an ancestor of node i. The notation i ⊀ j implies that node jis not an ancestor of node i. Node i is said to be the descendant of node j if j is anancestor of node i. A pair of nodes, i and j, are said to be related if one of them isthe ancestor of the other; otherwise they are said to be unrelated.

Figure 8.1 shows a taxonomy for graphs and digraphs adapted from Ramoni[137]. We briefly describe the left branch of this classification hierarchy in increas-ing order of specialization [38, 184]:

Directed(digraph) Undirected

Connected Disconnected

Acyclic(DAG) Cyclic

Simplyconnected(polytree)

Multiplyconnected

Simple tree(tree) Serial-chain

a

a

a

a

aa

a

b

b

b

b

bb

b

c

c

c

cc

c

d

d

d

dd

d

e

e

e

e

ee

e

Graph

Fig. 8.1 Hierarchical classification of graphs and digraphs

1. A connected digraph is a digraph such that there is an undirected path connect-ing any pair of nodes, i.e., it is a digraph without disjoint components.

2. A rooted digraph is a connected digraph with a single root node that is theancestor of every other node in the digraph. All edges connected to the root nodeare directed away from the root node. Thus, with r denoting the root node, wehave r� k for all nodes k in a rooted digraph.

3. A directed acyclic graph (DAG) is a connected digraph without any directedcycles, i.e., there is no directed path from any node back to itself. Thus, a node

8.1 Directed Graphs and Trees 137

cannot be its own ancestor in a DAG, i.e., k ⊀ k for any node k in the digraph.The first figure in Fig. 8.2 illustrates a DAG.

DAG Tree Canonicaltree

Strictlycanonical

tree

Canonicalserialchain

1 1

1 1 12

2 2

2

23 33

3

34 44 4

45 5

5 56 6

6 6

7

7 7

7

Fig. 8.2 Illustrations a digraphs with a DAG, a tree, canonical and strictly canonical treesand a serial-chain

4. A simply connected graph, or a polytree (also known as singly connected net-works), is a DAG in which there is at most one directed path between any pair ofnodes. For such digraphs, if nodes i and j are both ancestors of node k, then oneof the i and j nodes must be an ancestor of the other. Notationally, this conditionstates that if k≺ i and k≺ j, then either i� j or j� i. Removing an edge decom-poses a polytree into a pair of polytrees. A node in a polytree can have multipleparent nodes.

5. A simple tree, or tree, illustrated in the second figure in Fig. 8.2, is a polytreewhere a node has at most one parent node, i.e., ℘(k) contains at most one nodefor any node k. All trees have a unique root node. A tree is a rooted digraph,and is also referred to as a rooted tree or an arborescence. Removing an edgedecomposes a tree into a pair of trees.While a node in a tree can have at most one parent, there is no restriction on thenumber of children nodes. The branching structure implies that it is possible fornodes to be on different branches with no directed path connecting them. Thus,trees can have unrelated nodes, i.e., nodes that have no directed paths connectingthem. Another noteworthy fact is that, for a pair of nodes, i and j, while j� i =⇒i� j, the converse is not true in general. For a tree, we use the notation ℘(i, j) forthe closest ancestor node of a pair of nodes i and j.

6. A serial-chain is a tree where each node has at most one child. The last graphin Fig. 8.2 illustrates a serial-chain. Unlike trees, serial-chains have the strongerproperty that all node pairs are related, i.e., for any pair of nodes in the serial-chain, one of the nodes is necessarily an ancestor of the other.

7. An arborescence forest, or forest, is a collection of disjoint trees. Removingan edge from a tree converts it into a forest. Adding a common root node tothe independent trees in a forest converts them into a single tree. Conversely,removing the root node from a tree converts it into a forest.


A tree is said to be canonical if the index of a parent node is always greaterthan the index of its child node, i.e., ℘(k) > k for any node k. The third digraph inFig. 8.2 illustrates a canonical tree. Any tree can be converted into a canonical treewith a suitable renumbering of the nodes. The node numbering for a canonical treeis not unique since the canonical tree requirement imposes only a partial orderingon the node indices. The canonical numbering is, however, unique for a serial-chain.

A strictly canonical tree is a canonical tree such that each of the serial-chainsegments within it is canonical. The fourth digraph in Fig. 8.2 illustrates a strictlycanonical tree. Once again, any rooted tree can be converted into a strictly canonicaltree with a suitable renumbering of the nodes.

Every rooted digraph has a spanning tree, i.e., a tree that contains all the nodesin the digraph and whose edges belong to the digraph. The edges removed to converta rooted digraph into its spanning tree are referred to as cut-edges.

8.2 Adjacency Matrices for Digraphs

One way of representing the node/edge connectivity of a digraph is through anadjacency matrix for the digraph. For a digraph with n nodes, the adjacency matrixis an n×n square matrix. The only non-zero elements of an adjacency matrix arefor adjacent node pairs. Thus, the (i, j)th element of of an adjacency matrix is 1if and only if the jth node is a child of the ith node, and is 0 otherwise. With the“.” entries indicating 0 entries, adjacency matrices for the digraphs in Fig. 8.2 areshown in Fig. 8.3.

8.2.1 Properties of Digraph Adjacency Matrices

The adjacency matrix, S, of a digraph can be expressed as

S =

n∑j=1

∑k∈℘(j)

ek e∗j (8.1)

where ei denotes a vector of lengthn, containing all zeros except for the ith elementwhich has value 1, i.e.,

ei(r) =

{1 for i= r

0 otherwise= 1[i=r] (8.2)

1[<cond>] denotes the indicator function, whose value is 1 when the conditionspecified in its subscript is true, and whose value is 0 otherwise. Observe that

e∗j · ek = 1[j=k] (8.3)

8.2 Adjacency Matrices for Digraphs 139

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

. . . . . 1 .

. . . . . . .

. 1 . . . . .

1 . 1 . . . 1. . . 1 . . .

. . 1 . . . .

. . . . . . .

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

. . . . . 1 .

. . . . . . .

. 1 . . . . .

1 . 1 . . . 1. . . 1 . . .

. . . . . . .

. . . . . . .

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

. . . . . . .

. . . . . . .

1 . . . . . .

. 1 . . . . .

. . . . . . .

. . 1 1 1 . .

. . . . . 1 .

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

DAG Tree Canonical tree⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

. . . . . . .

1 . . . . . .

. . . . . . .

. . 1 . . . .

. . . . . . .

. 1 . 1 1 . .

. . . . . 1 .

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

⎛

⎜

⎜

⎜

⎝

. . . .

1 . . .

. 1 . .

. . 1 .

⎞

⎟

⎟

⎟

⎠

Strictly canonical tree Canonical serial-chain

Fig. 8.3 The adjacency matrices for the digraphs illustrated in Fig. 8.2

The number of eke∗j terms in (8.1) is precisely the number of edges in the digraph.Each eke∗j edge term is an n×n matrix, with a single non-zero element of value 1for the associated (k, j) edge. Key structural properties of an adjacency matrix, S,for a digraph are summarized below:

1. The non-zero entries in the jth column of S are for the ℘(j) parent nodes of thejth node. Thus, the product of S with the ith unit vector ei is a vector whosenon-zero entries are the parent nodes of the ith node. In particular, a node p is aroot node if and only if

S · ep = 0 (8.4)

In other words, root node columns of S have all zero elements.2. The non-zero entries in the ith row of S are for the �(i) child nodes of the ith

node. Thus, the product of S∗ with the ith unit vector, ei, is a vector whose non-

zero entries are the child nodes of the ith node. In particular, a node t is a tipnode, i.e., it has no children, if and only if

e∗t ·S = 0 (8.5)

In other words, tip node rows of S have all zero elements.3. Permutation matrices can be used to re-index the nodes in a digraph. Permutation

matrices are orthogonal, i.e., the transpose of a permutation matrix is its inverse.


With T denoting such a permutation matrix, and S the adjacency matrix of theoriginal digraph, the adjacency matrix of the digraph after re-indexing is TST∗.

8.2.2 Properties of Tree Adjacency Matrices

Since nodes in a tree can have at most one parent node, the adjacency matrix of atree digraph in (8.1) can be expressed in the following simpler form:

S =

n∑k=1

e℘(k)e∗k (8.6)

We now examine the properties of an adjacency matrix, S, for a tree digraph.

1. Since a node can have at most one parent in a tree, there can be at most onenon-zero entry in any column of S.

2. Only a single column, corresponding to the root node, is all zero in S.3. A row can have multiple non-zero elements.4. For canonical trees, S is always strictly lower-triangular.5. Reversing the direction of the edges in a tree typically destroys the tree character;

leaving only a polytree. The reversal process converts multiple children nodesinto multiple parent nodes. S

∗ is the adjacency matrix for the polytree.6. For strictly canonical trees, S has the structure shown in Fig. 8.4. It consists of the

adjacency matrix for canonical serial-chain branch segments, Si, along the diag-onal, along with off-diagonal Si,j, connector elements for the edges connectingthe serial-chain branch segments.

7. The kth power of the adjacency matrix, Sk, contains non-zero elements only

for paths of length k connecting the nodes. That is, the (i, j) element of Sk is

non-zero if and only if there is a directed path of length k from node i to node j.8. With n denoting the number of nodes in the tree, the nth power of the adjacency

matrix, Sn, is zero. This is because the length of any path in a tree with n nodes

is less than n. Thus S is a nilpotent matrix. In other words, the adjacency matrixfor a tree digraph is always nilpotent.

8.2.3 Properties of Serial-Chain Adjacency Matrices

We now review the properties of an adjacency matrix, S, for a serial-chain digraph.

1. Since a node can have at most one child in a serial-chain, its adjacency matrixcan have at most one non-zero element per row.

2. For a canonical serial-chain, only the first sub-diagonal of S is non-zero.3. For a canonical serial-chain, only the kth sub-diagonal of S

k contains non-zeroelements. Each additional power shifts the sub-diagonal one level lower.

8.3 Block Weighted Adjacency Matrices 141

S

S

S

S

S

S4,1 S4,2

S5,3 S5,4

Branch

0 00 0

0 0 0

00

0

00

0

0

0

0

11

1

2

2

2

33

3

44

4

5

5

5

Fig. 8.4 Structure of the S adjacency matrix for a strictly canonical tree topology system

4. A reversed serial chain is once again a serial-chain.

8.3 Block Weighted Adjacency Matrices

Adjacency matrices have 1 or 0 scalar entries for the node pair of each edge ina digraph. We now generalize these adjacency matrices to define block-weightedadjacency (BWA) matrices. Instead of scalars, BWA matrices contain weightmatrix elements for the node pair of each edge. First, each tree node, j, is assigneda weight dimension,mj, and its parent edge is assigned a non-zero weight matrix ofdimensionm℘(j) ×mj, denotedw(℘(j), j). The (k, j)th block-matrix element of aBWA matrix, SW , is thus defined as

SW(k, j) =

{w(k, j) if k = ℘(j)

0 otherwise∈Rmk×mj (8.7)

The standard adjacency matrix, S, in (8.1) is a special case of a BWA matrix, whereall the node dimensions are 1, and the edge weights are the scalar 1. SW has non-zero block-entries only for node pairs corresponding to edges in the digraph. It iseasy to verify from the definition of the block-elements in (8.7) that SW is a well-


defined square matrix of dimension

N�=

n∑k=1

mk (8.8)

We extend the notion of the ek unit vectors in (8.2), to the BWA context by assigningthem block matrix entries as follows:

ek(i) =

{Imk

if i= k

0mi×mkotherwise

(8.9)

Thus, the ek unit “vector” is a matrix of dimension N×mk, with n block matrixelements. The new version of (8.3) is

e∗j · ek =

{Imj

if j= k

0mj×mkotherwise

(8.10)

Using the unit vectors, and the component definitions in (8.7), SW can be expressedas:

SW =

n∑j=1

∑k∈℘(j)

ek w(k, j) e∗j (8.11)

Equation (8.11) is a generalization of (8.1) for BWA matrices.

8.4 BWA Matrices for Tree Digraphs

Analogous to (8.6), the BWA matrix expression in (8.11) has the following simplerform for tree digraphs:

SW =

n∑k=1

e℘(k)w(℘(k),k)e∗k (8.12)

The following lemma provides an expression for the block elements of SkW for tree

digraphs.

Lemma 8.1 Elements of SkW for a tree digraph.

For a tree digraph, SkW , the kth power of the SW BWA matrix, contains non-

zero block-elements only for node pairs connected by directed paths of length k.The value of its block-element is the product of the k weights for the edges along thepath, i.e., the non-zero elements are of the form

SkW

(

℘k(i),i)

= w(

℘k(i),℘k−1(i))

∗·· ·∗w(℘(i),i)∈Rm℘k(i)

×mi (8.13)

8.4 BWA Matrices for Tree Digraphs 143

Here, ℘k(i) denotes the kth ancestor of the ith node.Proof: Let us illustrate the proof for k = 2. S

2W is given by

S2W

8.12=

⎛

⎝

n∑j=1

e℘(j)w(℘(j), j)e∗j

⎞

⎠∗(

n∑i=1

e℘(i)w(℘(i), i)e∗i

)

8.3=

n∑j=1

n∑i=1

e℘(j)w(℘(j), j)1[j=℘(i)]w(℘(i), i)e∗i

=

n∑i=1

e℘2(i)w(

℘2(i),℘(i))∗w(℘(i), i)e∗i

The (℘2(i), i) block-element of S2W is thus,

S2W

(

℘2(i), i)

=w(

℘2(i),℘(i)) ∗w(℘(i), i)

which establishes (8.13) for S2W . Continuing in a similar vein, (8.13) can be estab-

lished for arbitrary k.

8.4.1 The 1-Resolvent of Tree BWA Matrices

The following lemma establishes the nilpotency of SW for tree digraphs.

Lemma 8.2 Nilpotency of tree BWA matrices.The nth power of a tree BWA matrix, S

nW , is zero, where n denotes the number of

nodes in the system. Hence, the BWA matrix for a tree digraph is a nilpotent matrix.Proof: Lemma 8.1 states that the non-zero entries of S

nW correspond to node pairs

connected by paths of length n. Since paths can be of length at most n− 1 in a treewith n nodes, it follows that S

nW is the zero matrix.

In matrix theory, the resolvent of a matrixA, is defined as the (λI−A)−1 matrixfor a scalar λ. We use the term, 1-resolvent, to denote the specific resolvent withλ = 1. The following lemma derives an explicit expression for the 1-resolvent of atree BWA matrix.

Lemma 8.3 The 1-resolvent of a tree BWA matrix.The 1-resolvent, (I− SW)−1, of a tree BWA matrix exists and can be expressed asthe following disjoint sum of its powers:

(I−SW)−1 = I+SW +S2W + · · ·+S

n−1W (8.14)


Proof: Lemma 8.2 states that the adjacency matrix of a tree, SW , is nilpotent.(8.14) follows by applying the expression from Lemma A.1 for the 1-resolvent of anilpotent matrix to SW .

Lemma 8.3 is an important result. It provides an explicit expression for the1-resolvent of the SW BWA matrix for tree systems. For cyclic digraphs, i.e., oneswith directed loops, the 1-resolvent is not defined because SW is not nilpotent and(I−SW) is non-invertible. This topological difference has significant implicationsfor the dynamics properties of tree versus cyclic digraph multibody systems. This isdiscussed in more detail in Sect. 9.6.2.

Now we introduce new notation that better reflects the intimate relationship be-tween a BWA matrix and its 1-resolvent matrix for a tree digraph. With A denotingthe 1-resolvent matrix for a tree, we will use the notation EA to denote its associ-ated BWA matrix. Thus, the relationship between the earlier notation and the newnotation is as follows:

A ≡ (I−SW)−1 and EA ≡ SW = I−A−1 (8.15)

The following lemma describes the expression for the block elements of a1-resolvent matrix.

Lemma 8.4 Elements of a 1-resolvent matrix A.The (k, j) element of the 1-resolvent matrix of a tree, A = (I−EA)−1, is given by

A(i, j) =

⎧⎪⎪⎪⎨⎪⎪⎪⎩

0mi×mjfor i� j

Imifor i = j

w(i, j) for i = ℘(j)

w(

℘k(j),℘k−1(j))∗ · · · ∗w(℘(j), j) for i = ℘k(j)

(8.16)

Proof: Equation (8.16) follows from the expression for (I−EA)−1 in (8.14), to-gether with Lemma 8.1 which describes the elements of the powers of EA.

Thus, A has identity matrices along the diagonal. The non-zero block elementscorrespond to (i, j) node pairs where node i is an ancestor of node j. For such cases,the block element is the product of the weight matrices associated with each ofthe edges in the path from i to j. From (8.16), it is clear that the A 1-resolvent issparse, with its sparsity structure directly reflecting the topological structure of theassociated tree.

Remark 8.1 Structure of canonical serial-chain EA and A matrices.Let us now examine the structure of the 1-resolvent matrix A for a canonical serial-chain digraph. We assume for this example that the serial-chain has four nodes, andthus E4

A= 0. Using (8.14), A is given by the following disjoint sum:

A = I+EA +E2A

+E3A

8.4 BWA Matrices for Tree Digraphs 145

The non-zero elements of the above matrices are as shown below (with “·” entriesindicating zero blocks and the X entries the non-zero ones):

EA =

⎛

⎜

⎜

⎜

⎝

· · · ·X · · ·· X · ·· · X ·

⎞

⎟

⎟

⎟

⎠

, E2A

=

⎛

⎜

⎜

⎜

⎝

· · · ·· · · ·X · · ·· X · ·

⎞

⎟

⎟

⎟

⎠

, E3A

=

⎛

⎜

⎜

⎜

⎝

· · · ·· · · ·· · · ·X · · ·

⎞

⎟

⎟

⎟

⎠

EA has non-zero block entries only along its first sub-diagonal. Each subsequentpower of EA shifts the non-zero sub-diagonal lower, until it vanishes entirely for thenth power. tHUS, A has the following structure:

A =

⎛

⎜

⎜

⎜

⎝

I · · ·X I · ·X X I ·X X X I

⎞

⎟

⎟

⎟

⎠

A is lower-triangular, with all of the diagonal and lower-triangular block-elementsbeing non-zero. All of these terms are non-zero because all nodes in serial-chainsare related, i.e., there is a directed path between all node pairs.

This example shows that, for a canonical serial-chain, all of the lower-triangularelements of A are non-zero. The following example looks at the structure of a1-resolvent matrix for a canonical tree.

Remark 8.2 Structure of canonical tree EA and A matrices.The sparsity structure of the EA and A matrices for the canonical tree in Fig. 8.2 isshown below. The “·” entries indicate zero block-elements.

EA =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

· · · · · · ·· · · · · · ·X · · · · · ·· X · · · · ·· · · · · · ·· · X X X · ·· · · · · X ·

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

and A =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

I · · · · · ·· I · · · · ·X · I · · · ·· X · I · · ·· · · · I · ·X X X X X I ·X X X X X X I

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

These matrices illustrate that, for canonical trees, the EA matrix is strictly lower-triangular, and A is lower-triangular. However, some of the lower-triangular blocksof A are zero, in contrast with the all non-zero case for canonical serial-chains.The zero terms correspond to the unrelated nodes in the tree, i.e., node pairs that


are not connected by a path. Section 14.3 takes a more detailed look at the sparsitystructures of the EA and A matrices.

This example shows that there is significant sparsity in the lower-triangularblocks of A for a tree. The sparsity is a function of the branching topological struc-ture of the tree. Serial-chain systems represent the most dense topological structuresince all of their nodes are related. Trees, on the other hand, can have unrelatednodes, i.e., nodes with no directed paths connecting them. This decreased connec-tivity of the nodes is reflected in the sparsity of A.

The following lemma highlights the semi-group properties of the block elementsof A.

Lemma 8.5 Semi-group property of A(i, j) elements.For a tree, let i, j, and k denote nodes, where the ith node is an ancestor of the jthnode, and the kth node is on the path connecting them. Then, the block-elements ofthe A 1-resolvent satisfy the following relationship:

A(i, j) = A(i,k)A(k, j) ∀ k : i� k� j (8.17)

This property is also known as the semi-group property for the elements of a1-resolvent matrix.Proof: Let i = ℘m(j) for some m, and k = ℘r(j) for some r < m. Let l denotethe child node of k that is on the path to node j. From the (8.16) expression for theblock elements of A, we have

A(i, j)8.16= w

(

℘m(j),℘m−1(j))∗ · · · ∗w(℘(j), j)

=[

w(

℘m(j),℘m−1(j))∗ · · · ∗w(℘(k),k)

]∗ [w(k, l) ∗ · · · ∗w(℘(j), j)]

8.16= A(℘m(j),k)∗A(k, j) = A(i,k)∗A(k, j)

Define the A matrix derived from the A 1-resolvent matrix as follows:

A�= A− I (8.18)

A is strictly lower-triangular for canonical trees. The following lemma establishessome basic properties of A.

Lemma 8.6 The A spatial operator.For a 1-resolvent A, the following identities hold:

A = A− I = EAA = AEA (8.19)

Proof: For any matrix X, such that (I −X) is invertible, the following matrixidentity holds:

X(I−X)−1 = (I−X)−1X= (I−X)−1 − I

8.5 Similarity Transformations of a Tree BWA Matrix 147

With X= EA, the above equation, together with (8.18), directly lead to (8.19).The following exercise extends the adjacency matrix properties in (8.4) and (8.5)

to BWA matrices and their 1-resolvents.

Exercise 8.1 Root/tip nodes and BWA matrices.

1. A node r is a root node of the tree if and only if

EA · er = 0, A · er = er (8.20)

2. A node t is a tip node of the tree, i.e., it has no children nodes, if and only if

E∗A · et = 0, A

∗ · et = et (8.21)

The following remark describes properties of the product of a general matrix, anda diagonal matrix, that we will use in the forthcoming sections.

Remark 8.3 Matrix products.Recall that any block-matrix, Y, can be expressed as

Y =

n∑j=1

n∑k=1

ejY(j,k)e∗k

Let X�= diag

{X(i)

}n

i=1and Z

�= diag

{Z(i)

}n

i=1denote compatible block-

diagonal matrices. Then it is easy to verify that

XY =

n∑j=1

n∑k=1

ejX(j)Y(j,k)e∗k

and YZ=

n∑j=1

n∑k=1

ejY(j,k)Z(k)e∗k

(8.22)

While the discussion in this section has been devoted to BWA matrices and1-resolvents for tree digraphs, all of the properties and results extend directly toforests, i.e., to digraphs consisting of a set of disjoint trees.

8.5 Similarity Transformations of a Tree BWA Matrix

We now examine the effect of applying similarity transformations to a BWA matrix.


Lemma 8.7 Similarity transformation of a BWA matrix.

Let T denote an invertible matrix and EA a tree BWA matrix. Let Y�= TEAT

−1

denote a similarity transformation of EA by the T matrix. Then Y is nilpotent, andits 1-resolvent is given by the following similarity transformation:

(I−Y)−1 = TAT−1 (8.23)

Proof: Since EA is nilpotent, we have

Yn = (TEAT−1)n = T(EA)nT−1 = 0

Hence, Y is nilpotent. Also,

(I−Y)−1 = (I−TEAT−1)−1 = T(I−EA)−1T−1 = TAT−1

This establishes (8.23).The above lemma provides an expression for the 1-resolvent of the matrix ob-

tained by applying a similarity transformation to a BWA matrix. However, there isno guarantee that the transformed matrix, Y, retains the block partitioned structureof the original BWA matrix. Generally, the transformed Y matrix will not be a BWAmatrix for the tree.

8.5.1 Permutation Similarity Transformations

A special case, where the BWA property is preserved under similarity transforma-tions, occurs when the transformation matrix is a permutation matrix used to re-index the nodes in the tree. For such a permutation matrix, denoted T , we know that

T−1 = T∗. The transformed matrix, EB

�= TEAT

−1 is indeed a BWA matrix for thetree, and its 1-resolvent B is given by:

EB = TEAT∗ and B

�= (I−EB)−1 8.23

= TAT∗ (8.24)

In the multibody context, such permutation transformations relate the equations ofmotion obtained using different body numbering schemes. In particular, any tree canbe converted into a canonical tree with an appropriate re-labeling of the nodes. Thisimplies that the EA and A matrices for any tree are related to the lower-triangularversions for the corresponding canonical tree via permutation matrices.

8.5 Similarity Transformations of a Tree BWA Matrix 149

8.5.2 Similarity-Shift Transformations

Another type of similarity transformation, referred to as a similarity-shift transfor-mation, that preserves the BWA property is discussed next. Let ΔC be an invertible,block-diagonal matrix defined as follows:

ΔC

�= diag

{ΔC(k)

}n

k=1where ΔC(k) ∈Rmk×mk (8.25)

Let B(℘(k),k) denote new weight matrices defined from the original A(℘(k),k)weight matrices as follows:

B(℘(k),k) = ΔC(℘(k)) A(℘(k),k) Δ−1C

(k) (8.26)

Let EB denote the BWA matrix with the B(℘(k),k) weight matrices. The followinglemma establishes the relationship between the EA and EB BWA matrices and their1-resolvents.

Lemma 8.8 Similarity-shift transformation of a BWA matrix.Let EA and EB be BWA matrices whose weight matrices satisfy (8.26). Then the BWAmatrices EB and EA are related by the following similarity transformation:

EB = ΔC EA Δ−1C

(8.27)

Also, the 1-resolvent matrices, A = (I−EA)−1 and B = (I−EB)−1, are related bythe same similarity transformation:

B = ΔC A Δ−1C

(8.28)

Proof: Now

EB

8.26=

n∑k=1

e℘(k)ΔC(℘(k))A(℘(k),k)Δ−1C

(k)e∗k

8.22=

n∑k=1

ΔC e℘(k)A(℘(k),k)e∗kΔ−1C

= ΔC

n∑k=1

e℘(k)A(℘(k),k)e∗kΔ−1C

= ΔC EA Δ−1C

This establishes (8.27). Further application of Lemma 8.7 leads to (8.28).In the multibody context, similarity-shift transformations define the relationship

between equations of motion associated with different choices of body frames.


8.6 Multibody System Digraphs

Digraphs provide natural mathematical constructs for describing the topology andconnectivity of bodies in a multibody system. They have been used for the system-atic formulation of the equations of motion [123,164,185], as well as for kinematicalanalysis [189].

We define the standard digraph associated with a multibody system as one withthe inertial frame as the root node, and all the links in the system as the remainingnodes in the digraph. Thus, an n-link multibody system has a digraph with n+ 1nodes.

The edges in the digraph are defined by the motion constraints among the bodies,and between the bodies and the inertial frame. Thus, each hinge is represented by anedge, with the edges orientated from the inboard to the outboard body. Additionaledges are assigned to other non-hinge motion constraints in the system. All motionconstraints with respect to the inertial frame are defined so that edges from theinertial frame node to the link nodes are directed away from the inertial frame rootnode. These assignments result in a rooted digraph representation for the multibodysystem. Figure 8.5 illustrates the correspondence between the bodies and hinges in aserial-chain multibody system and the standard, serial-chain digraph for the system.The convention is to depict the inertial frame node as unfilled, and the edges fromthe node as dashed lines. Multibody systems are classified as follows, based on the

Serial-chainmultibodysystem

Serial-chaindigraph

Body node

Base-body node

Hinge edge

Inertial frame node

Fig. 8.5 The relationship between a serial-chain multibody system and its standard,serial-chain digraph. The inertial frame nodes are shown as unfilled nodes, while theedges from the node are shown as dashed lines

topology of their standard digraph:

• Systems with tree standard digraphs are referred to as tree-topology systems.

8.6 Multibody System Digraphs 151

• Systems with serial-chain standard digraphs are referred to as the familiarserial-chain systems. They are special cases of tree-topology systems.

• Systems with non-tree standard digraphs are referred to as closed-chain orconstrained systems. These digraphs can have directed cycles and/or multiply-connected nodes. Recall that every rooted digraph can be decomposed (non-uniquely) into a spanning tree together with a set of cut-edges. A decompositioninto a spanning tree with n+ 1 nodes and a set of cut-edges is often used whenworking with closed-chain systems.

Removing the inertial root node converts the spanning tree into a forest. We willrefer to this forest as the SKO-forest for the system. The SKO-forest has just nnodes, matching the number of bodies in the system. Nodes corresponding to base-bodies in the multibody system are root nodes in the SKO-forest. We will makeextensive use of n-node SKO-forests since they allow us to work with adjacencymatrices of dimension n instead of dimension n+ 1. There is no information lossbecause the spanning tree can be fully recovered by adding the inertial frame rootnode and its edges to the root nodes in the SKO-forest. The inertial frame node is,in reality, redundant in the spanning tree of a multibody system since it is alwayspresent as the root node.

Figure 8.6 illustrates several examples of multibody systems and their associatedstandard digraph representations, as described below:

1. A serial-chain system with a serial-chain digraph.2. A tree-topology system consisting of a pair of independent serial manipulators.

The resulting digraph is a tree.3. A tree-topology system with branching reflected in its tree digraph.4. A closed-chain four-bar linkage with an internal loop constraint. Based on the

directionality of the constraint hinge, the digraph contains a directed cycle or ismultiply-connected.

5. A closed-chain pair of serial-manipulators whose end-effectors are constrained.The digraph for this system is a multiply-connected.

6. A closed-chain serial manipulator with constrained end-effector. The digraph ismultiply-connected.

In later chapters, we will encounter non-standard digraphs for multibody systemswhere the bodies and digraph nodes are not in one-to-one correspondence with eachother.

8.6.1 BWA Matrices and Serial-Chain, Rigid Body Systems

In this section, we use a canonical serial-chain multibody system to help estab-lish ties between BWA matrices and their 1-resolvents, and multibody dynamicsformulations. Consider an n-link rigid body canonical serial-chain system, withlinks numbered so that the tip link is link 1, and the base-body is the nth link.


(3) Torso with arms (tree digraph)

(4) Mechanism with a four-bar linkage (digraphwith directed cycle and a multiply-connecteddigraph)

(1) A serial manipulator(serial-chain digraph)

(2) Pair of independent serial manipulators (treedigraph)

(6) Serial manipulator withconstrained end-effector(multiply-connected digraph)

(5) Pair of constrained serial manipulators (multiply-connected digraph)

Fig. 8.6 Examples of general multibody systems and their standard digraph representations

Its digraph is a strictly canonical serial-chain with the parent/child relationship de-fined by ℘(k) = k+ 1.

Recall the following expression for the link spatial velocities from (3.34):

V = E∗φV+H∗θ (8.29)

8.6 Multibody System Digraphs 153

with the spatial operator Eφ in (3.30) defined as

Eφ =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0

φ(2,1) 0 . . . 0 00 φ(3,2) . . . 0 0...

.... . .

......

0 0 . . . φ(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(8.30)

8.6.1.1 Eφ Is a BWA Matrix

Equation (8.30) can restated as

Eφ =

n∑k=1

ek+1φ(k+ 1,k)e∗k =

n∑k=1

e℘(k)φ(℘(k),k)e∗k (8.31)

Comparing with (8.12), we see that Eφ in (8.31) has the structure of a BWA matrixfor a tree digraph with n nodes. In fact, Eφ is a BWA matrix for the SKO-forestassociated with the canonical serial-chain system. The weight dimensions for theBWA matrix are uniformly of size 6, and the edge weight matrices are 6×6 matricesdefined as

w(℘(k),k) =w(k+ 1,k)�= φ(k+ 1,k) = φ(℘(k),k) (8.32)

Since Eφ is a BWA matrix, by Lemma 8.3, its 1-resolvent, φ = (I − Eφ)−1, iswell-defined, and is given by

φ=

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0φ(2,1) I . . . 0

......

. . ....

φ(n,1) φ(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

∈R6n×6n (8.33)

This expression is identical to the one forφ in (3.36). Thus, we have established thatthe Eφ and φ spatial operators for canonical serial-chain multibody systems are infact BWA and 1-resolvent matrices, respectively, for the SKO-forest associated withthe system. The strictly lower-triangular and lower-triangular structures of the Eφ

and φ matrices, respectively, are consistent with their association with canonicalserial-chain digraphs.


8.6.2 Non-Canonical Serial-Chains

For non-canonical serial-chains, the (k+1)th link is not necessarily the parent bodyof the kth link. So a natural question is whether the Eφ and φ spatial operatorsfor non-canonical serial-chains continue to be BWA and 1-resolvent matrices? Theanswer is in the affirmative. We simply need to discard the implicit assumption thatthe parent body of the kth link is the (k+1)th link, and switch to the more general℘(k) notation for the parent link. The generalization of (3.19b) for non-canonicalserial-chain systems is:

V(k) = φ∗(℘(k),k)V(℘(k))+H∗(k)θ(k) (8.34)

The velocity relationship in (8.29) continues to hold, with Eφ defined by the moregeneral second half of (8.31). Thus, Eφ is once again a BWA matrix. Indeed, therest of the development in Sect. 5.1 on page 75, leading to the equations of motionin (5.23) and (5.24), continues to apply. However, since the system is non-canonical,neither Eφ nor φ in (8.30) and (8.33), respectively, are lower-triangular. Thus, eventhough the component-level equations and the operator structure depend on the bodyindexing scheme, the BWA and 1-resolvent properties of the Eφ and φ spatial oper-ators, and the operator forms of the equations of motion, remain intact! In the nextsection, we will encounter this theme again for tree-topology systems.

8.7 BWA Matrices and Tree-Topology, Rigid Body Systems

The key difference between serial-chain and tree-topology multibody systems isthat, in a tree system, bodies can have multiple children bodies. The standard di-graph for an n-links tree-topology multibody system is a tree with (n+1) nodes, asillustrated in Fig. 8.7. We make no specific assumptions about the indexing schemefor the bodies. However, as usual, structural properties are easier to visualize whencanonical indexing schemes are used.

Examining the kinematic velocity relationships across the links, the inter-linkvelocity relationship shown in (8.34) continues to apply to bodies in a tree-topologysystem:

V(k) = φ∗(℘(k),k)V(℘(k))+H∗(k)θ(k) (8.35)

That is, the spatial velocity of the kth link can be expressed as the sum of therigidly propagated spatial velocity of the parent body and the relative spatial veloc-ity, H∗(k)θ(k), across the kth hinge. Defining the V and θ stacked vectors and theH spatial operator in the same way as in Sect. 3.4, it is easy to verify that the fol-lowing operator expression is a system-level rearrangement of the component-levelvelocity relationship in (8.35):

V = E∗φV+H∗θ (8.36)

8.7 BWA Matrices and Tree-Topology, Rigid Body Systems 155

Treetopologymultibodysystem

Treedigraph

Body node

Base-body node

Hinge edge

Inertial frame node

Fig. 8.7 The correspondence between a tree-topology multibody system and its stan-dard tree digraph

with Eφ defined as

Eφ =

n∑k=1

e℘(k)φ(℘(k),k)e∗k (8.37)

The Eφ spatial operator has the (8.12) form of a BWA matrix for the SKO-forestassociated with the system. The φ(℘(k),k) matrices are the 6×6 weight matrices.Since the SKO-forest is a forest, Eφ is nilpotent and, from Lemma 8.3, it has a welldefined 1-resolvent, φ= (I−Eφ)−1. Hence, (8.36) can be transformed into

(I−E∗φ)V =H∗ θ and V = φ∗H∗ θ (8.38)

This operator expression is identical to the one in (8.29) for serial-chain systems.

8.7.1 Equations of Motion for Tree Topology Systems

To develop the equations of motion for the tree system, we once again start with thefree-body equations of motion of the kth link, as was done in (5.1) for serial-chainsystems. The key difference is that we need to take into consideration the f(.) spatialinteraction forces from all the children bodies, instead of from just a single childbody, as is the case for serial-chain systems. The following is the generalization ofthe force balance expression from (5.1) extended to tree-topology systems:


f(k)−∑

∀j∈�(k)

φ(k, j)f(j) =M(k)α(k)+b(k) (8.39)

Rearranging the above leads to the following analog of (5.4):

f(k) =∑

∀j∈�(k)

φ(k, j)f(j)+M(k)α(k)+b(k) (8.40)

Switching to the system-level stacked vector form of (8.40), we obtain

f = Eφf+Mα+b (8.41)

Continuing in this mode, the following expressions summarize the operator expres-sions for tree-topology systems:

V8.36= E∗

φV+H∗θ

α= E∗φα+H∗θ+a

f8.41= Eφf+Mα+b

T =Hf

(8.42)

These expressions are identical to the corresponding expressions in (5.22) for serial-chain systems. Using φ = (I−Eφ)−1, these expressions can be transformed fromimplicit ones into the following explicit operator expressions:

V = φ∗H∗ θ

α= φ∗(H∗ θ+a)

f = φ(Mα+b)

T =Hf

(8.43)

These expressions are identical to the corresponding expressions in (5.23) for serial-chain systems. Combining the expressions in (8.43) leads to

T = M(θ)θ+C(θ, θ) (8.44)

where

M(θ) =HφMφ∗H∗ ∈RN×N and C(θ, θ)�= Hφ(Mφ∗a+b) ∈RN (8.45)

M ∈ RN×N denotes the mass matrix for the tree-topology system, and C ∈ RN

is the vector of velocity dependent nonlinear Coriolis and gyroscopic velocity de-pendent terms. These operator expressions are identical to the ones in (5.25) forserial-chain systems. Adopting the terminology from the serial-chain case, we referto the expression for M in (8.45) as the Newton–Euler Factorization of the massmatrix.

8.7 BWA Matrices and Tree-Topology, Rigid Body Systems 157

We thus conclude that, even though the component-level relationships and therespective operator structures are quite different between canonical and non-canonical serial-chain systems and tree-topology systems, the BWA and 1-resolventmatrix properties of the Eφ and φ spatial operators persist, and the operator levelform of the equations of motion and the mass matrix, remarkably, remain un-changed! The connection between BWA matrices and multibody spatial operatorswas originally described in [75, 76].

Chapter 9SKO Models

For canonical serial-chain systems, spatial operator techniques have been used toestablish important analytical results, such as decompositions and factorizations ofthe mass matrix, and an operator expression for its inverse. This analytical ground-work led to low-order computational algorithms, such as for inverse dynamics,forward dynamics, and for computing the mass matrix.

Chapter 8 revealed that the Eφ and φ spatial operators are BWA and 1-resolventmatrices for serial-chain, rigid-link multibody systems. We also found that verysimilar spatial operator level formulations, along with the associated BWA and1-resolvent matrix properties, extended virtually unchanged to non-canonicalserial-chains and to tree-topology rigid body systems. This remarkable consistencypersists despite significant differences in the component structure of the corre-sponding Eφ andφ spatial operators for these different systems. BWA matrices and1-resolvent matrices will start to play a central role in our study of the dynamics ofmultibody systems. We will henceforth refer to multibody system spatial operatorsthat are BWA matrices (such as Eφ) as spatial kernel operators (SKO), and totheir associated 1-resolvent matrices (such as φ) as spatial propagation operators(SPO).

A natural question that arises is whether the spatial operator analytical and algo-rithmic techniques developed for canonical serial-chain rigid body systems carry-over to tree-topology rigid body systems as well? Indeed, they do, and with verymodest changes. In fact, these extensions are not limited to rigid-body tree-topologysystems either, but are more broadly applicable.

To establish their generality, we use the SKO formulation as a point of departure.We will see that the spatial operator analytical techniques and algorithms developedfor serial-chain systems originate from the SKO nature of the dynamics formula-tion, rather than from the specific, canonical, serial-chain, or rigid-body nature ofthe system. This motivates the use of SKO techniques as a general approach for for-mulating the dynamics of multibody systems, with the assurance that the powerfulresults from SKO formulations will automatically be available for further analysisand algorithm development.


160 9 SKO Models

9.1 SKO Models

Central to SKO formulations are the development of SKO models for multibodysystems. These models are formally defined in the following section, and their prop-erties are explored in the rest of the chapter.

9.1.1 Definition of SKO Models

An SKO model for an n-links tree-topology multibody system consists of the fol-lowing:

1. A tree digraph reflecting the bodies and their connectivity in the system.2. An EA SKO operator and associated SPO operator, A.3. A full-rank block-diagonal, joint map matrix operator,H.4. A block-diagonal and positive-definite spatial inertia operator, M.5. Stacked vectors: θ denoting independent generalized velocities, T the general-

ized forces, V the node velocities, α the node accelerations, f the inter-nodeforces, a the node Coriolis accelerations, b the node gyroscopic forces the sys-tem, N the number of degrees of freedom, and the equations of motion defined as:

V = A∗H∗ θ

α= A∗(H∗ θ+a)

f = A(Mα+b)

T =Hf

(9.1)

Thus,

T = M(θ)θ+C(θ, θ) (9.2)

where

M(θ)�= HAMA

∗H∗ ∈RN×N and C(θ, θ)�= HA(MA

∗a+b) ∈RN (9.3)

M is the symmetric and positive-definite mass matrix for the tree system and C isthe vector of nonlinear Coriolis and gyroscopic velocity dependent terms. (9.3)is the Newton–Euler operator factorization of the mass matrix.

To maximize generality, no assumptions have been made about the specific natureof the weight matrices in the SKO operators, about the component elements of HandM, or that the tree digraph is the standard digraph in an SKO model.

9.1 SKO Models 161

9.1.2 Existence of SKO Models

The SKO formulations derived in Chap. 8 for rigid-body, serial-chain and tree-topology, multibody systems are examples of SKO models with Eφ and φ as therespective EA SKO and A SPO operators. The existence of an SKO model does notdepend on any exceptional or unusual features of tree-topology multibody systems.Indeed, SKO models can be viewed as a refinement of the models obtained from theclassic, and widely used, Kane’s method for developing multibody dynamics mod-els [95, 96]. Central to Kane’s method is the identification of partial velocities thatdefine the mapping from generalized velocities to body velocities. The elements ofthe A

∗H∗ matrix product of an SKO model are precisely Kane’s partial velocities.This is an important connection between these modeling techniques.

While the partial velocity elements are adequate for formulating dynamics mod-els using Kane’s method, the important SPO property is unfortunately lost once theA∗H∗ partial velocities product is formed. The SPO nature of the operator A plays

a crucial role in analysis and algorithm development. Therefore, SKO models retainthe more fundamental (H,A,M) family of operators to not only define the (9.2) dy-namics model, but also to derive important analytical and algorithmic techniques.We will, at times refer to an SKO model by its (H,A,M) triplet of operators.

Section 9.6 describes a systematic process for developing SKO models for multi-body systems; in later chapters, we will apply this process to develop SKO modelsfor a variety of systems.

9.1.3 Generalizations of SKO Models

To highlight the possible areas of generalization of SKO models, we contrast, below,the properties of BWA matrices with those of the familiar SKO operators for serialand tree-topology rigid body systems.

• BWA weight matrices are not limited to 6×6φ(℘(k),k) rigid-body transforma-tion matrices.

• BWA weight matrices can be non-square.• BWA weight matrices can be of non-uniform size across edges.• BWA weight matrices can be singular.

In general, SKO weight matrices can, and will, deviate in all of these respects fromthose for rigid body systems. We will see that the wide variability of the multibodysystems is embodied in the definitions of their SKO model digraphs and, in thespecific definitions of the weight matrices and operator component values.

We now mention two cautionary points concerning SKO models.

Definition of body nodes: The standard digraph for a multibody system assignsa node to each physical body in the system. This one-to-one correspondencebetween physical bodies and digraph nodes has been used to develop the SKO

162 9 SKO Models

models for tree-topology systems. However, this one-to-one correspondence isnot a requirement for an SKO model. We will encounter cases where multiplephysical bodies are associated with a single node in the digraph, as well as theconverse, where multiple nodes are assigned to a single body! Since the majorityof models will indeed be using the standard digraphs, we will use the nodesand bodies terminology interchangeably, and will highlight the exceptional caseswhen we encounter them.

Spatial operator compatibility: By construction, spatial operators have blockstructure derived from a combination of the underlying model digraph and thenode weight dimensions. However, SKO models for a system are not unique, andmay differ in the choices of digraph nodes, edges and weight dimensions.Spatial operators and stacked vectors are said to be compatible when they areassociated with the same SKO model. Expressions composing spatial operatorsand stacked vectors are only meaningful when all the component terms are com-patible. Therefore, care is needed to avoid mixing spatial operators associatedwith different SKO models. The compatibility requirement, however, does notpreclude the definition of transformations that relate operators across differentSKO models.

In this section, and the following ones, we develop analytical results and algo-rithms for SKO models with no assumptions beyond those listed for the definitionof these models. Therefore, the results can be used for any system for which anSKO model is available. We will also apply these results to important dynamicsproblems. Several of the spatial operator techniques and algorithms for serial-chain,rigid body systems will be generalized to the broader class of SKO models. SKOmodels will also be used to study the dynamics of closed-chain multibody systemsin later chapters.

9.2 SPO Operator/Vector Products for Trees

In additional to the mathematical analysis supported by spatial operators, there is anintimate relationship between operator expressions and efficient recursive computa-tional algorithms. We begin by studying this relationship in detail in the followinglemma.

Lemma 9.1 Tips-to-base gather recursion to evaluate Ax.Given an SPO operator, A, and a compatible stacked vector, x, for a tree-topologysystem, the y(k) elements of y = Ax, satisfy the following parent/child recursiverelationship:

y(k) =∑

∀i∈�(k)

A(k, i)y(i)+x(k) (9.4)

9.2 SPO Operator/Vector Products for Trees 163

Based on this relationship, the elements of y can be computed using the followingO(N) tips-to-base gather recursive procedure:

⎧⎪⎪⎪⎨⎪⎪⎪⎩

for all nodes k (tips-to-base gather)

y(k) =∑

∀i∈�(k)

A(k, i)y(i)+x(k)

end loop

(9.5)

As will be customary, the steps in such gather recursions only apply to the bodynodes and not the inertial node in the digraph.Proof: The y= Ax expression implies that

y(k) =

n∑j=1

A(k, j)x(j)8.16=

∑∀j�k

A(k, j)x(j) (9.6)

Therefore,

y(k)9.6,8.17

=∑

∀i∈�(k)

A(k, i)∑∀j�i

A(i, j)x(j)+x(k)9.6=

∑∀i∈�(k)

A(k, i)y(i)+x(k)

This establishes (9.4). The procedure in (9.5) follows directly from this recursiveexpression.

The left side of Fig. 9.1 illustrates the flow of the recursive procedure in (9.5)for computing the y(k) elements. It starts at the tip bodies of the tree and proceedstowards the base-body, while gathering together values converging at the branches.The computational cost of this tips-to-base gather procedure is just of O(N) com-plexity, instead of the O(N2) complexity of a direct Ax matrix/vector product. Wewill encounter several applications later that use this lemma to efficiently evaluateoperator expressions involving products of SPO operators and stacked vectors. For acanonical serial-chain system, this gather algorithm simplifies to the one in Lemma3.1 on page 51.

Similarly, the x= A∗y product can be evaluated using a base-to-tips,O(N) scat-

ter computational algorithm described in the following lemma.

Lemma 9.2 Base-to-tips scatter recursion to evaluate A∗x.

Given an SPO operator, A, and a compatible stacked vector x, for a tree-topologysystem, the y(k) elements of y = A

∗x, satisfy the following parent/child recursiverelationship:

y(k) = A∗(℘(k),k)y(℘(k))+x(k) (9.7)

Based on this relationship, the elements of y can be computed using the followingO(N) base-to-tips scatter recursive procedure:

⎧⎪⎨⎪⎩

for all nodes k (base-to-tips scatter)

y(k) = A∗(℘(k),k)y(℘(k))+x(k)

end loop

(9.8)

164 9 SKO Models

y = Ax y = A∗x y( )y( )

y( )y( )

y( ) =

∀j∈ ( )

A( ,j)y(j)

+x( )

y( ) =

A∗(℘( ), )y(℘( ))

+x( )

Tips-to-basegatherrecursio

Base-to-tipsscatterrecursio

1 11 1

k

kkk

k

k

kk

kk knn

n nn n

Fig. 9.1 Tips-to-base gather and base-to-tips scatter recursions to evaluate Ax and A∗x,respectively for tree-topology systems

As will be customary, the steps in such scatter recursions only apply to the bodynodes and not the inertial nodes in the digraph. The quantities for the inertial noderequired in the initial step for the base-bodies in the scatter recursion are alwaysassumed to be zero.Proof: The y= A

∗x expression implies that

y(k) =

n∑j=1

A∗(j,k)x(j) 8.16

=∑∀j�k

A∗(j,k)x(j) (9.9)

Therefore,

y(k)9.9,8.17

= A∗(℘(k),k)

∑∀i�℘(k)

A∗(i,℘(k))x(i)+x(k)

9.9= A

∗(℘(k),k)y(℘(k))+x(k)

This establishes (9.7). The recursion in (9.8) is a direct consequence of thisrelationship.

The right side of Fig. 9.1 illustrates the flow of the recursive procedure in (9.8)for computing the y(k) elements. It starts at the base-body of the tree and pro-ceeds towards the tips, while scattering the values onto diverging branches. Thecomputational cost of this procedure is again just of O(N) complexity, instead ofthe O(N2) complexity of a direct A

∗x matrix/vector product. We will encounter

9.2 SPO Operator/Vector Products for Trees 165

several applications that use this lemma to efficiently evaluate products involvingthe transpose of an SPO operator and a stacked vector. For a serial-chain system,this scatter algorithm simplifies to the one derived in Lemma 3.2 on page 52.

Exercise 9.1 Recursive evaluation of Ax and A∗x.

Similar to (9.5) and (9.8), show that the elements of y = Ax satisfy the followingrecursive gather relationship:

y(k) =∑

∀j∈�(k)

A(k, j)[

y(j)+x(j)]

Additionally, show that the elements of y = A∗x satisfy the following recursive

scatter relationship:

y(k) = A∗(℘(k),k)

[

y(℘(k))+x(℘(k))]

Exercise 9.1 is a generalization of the serial-chain system algorithm describedpreviously in Exercise 3.7 on page 53 for the φx and φ∗x products.

Based on the results in this section, we can make the important observation thatthe serial-chain recursive algorithms for operator expressions can be easily gen-eralized to tree-topology systems by simply replacing tip-to-base recursions withtips-to-base gather recursions, and base-to-tip recursions with base-to-tips scatterrecursions.

9.2.1 SKO Model O(N) Newton–Euler Inverse Dynamics

We now apply the mapping of SPO and stacked vector products to develop an ef-ficient O(N) recursive algorithm for the inverse dynamics problem for a systemwith an SKO model. For the inverse dynamics problem, the θ generalized acceler-ations are assumed to be known, and the corresponding T generalized forces needto be computed. (9.1) and (9.2) define the operator expressions for this mapping forthe system. These operator expressions are precisely in the form of SPO and stackedvector products. Thus Lemmas 9.1 and 9.2 can be used to evaluate them using recur-sive scatter and gather algorithms. Algorithm 9.1 describes such an implementation.The algorithm begins with a base-to-tips scatter recursion that implement the op-erator expressions for V and α in (9.1). This is followed by a recursive tips-to-basegather recursion to evaluate the expressions for f and T in (9.1). This algorithm is ageneralization of the serial-chain inverse dynamics algorithm described previouslyin Algorithm 5.2 on page 89. The primary changes are the switch to scatter/gatherrecursions required for tree systems, and the use of the generic component weightmatrices for the SKO model.

166 9 SKO Models

Algorithm 9.1 Newton–Euler inverse dynamics algorithm for an SKO model⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩


V(k) = A∗(℘(k),k)V(℘(k))+H∗(k)θ(k)

α(k) = A∗(℘(k),k)α(℘(k))+H∗(k)θ(k)+a(k)

end loop

⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩


f(k) =∑

∀j∈�(k)

A(k, j)f(j)+M(k)α(k)+b(k)

T(k) =H(k)f(k)

end loop

(9.10)

9.3 Lyapunov Equations for SKO Models

This section studies SPO operator quadratic expressions that are related to forwardand backward Lyapunov equations for an SKO model.

9.3.1 Forward Lyapunov Recursions for SKO Models

Lemma 9.3 Structure of AXB∗.

Let A and B denote compatible SPO operators and X be a compatible

block-diagonal operator. Then the Z�= AXB

∗ product can be decomposed intothe following disjoint sum

Z= Y+ AY+YB∗ (9.11)

where Y is a block-diagonal operator satisfying the forward Lyapunov equation:

X= Y−EAYE∗B (9.12)

The Y(k) diagonal elements satisfy the recursive parent/child relationship:

Y(k) =∑

∀j∈�(k)

A(k, j)Y(j)B∗(k, j)+X(k) (9.13)

9.3 Lyapunov Equations for SKO Models 167

Based on this relationship, the Y(k) terms can be computed via the O(N)tips-to-base, forward Lyapunov gather recursion:

⎧⎪⎪⎪⎨⎪⎪⎪⎩


Y(k) =∑

∀j∈�(k)

A(k, j)Y(j)B∗(k, j)+X(k)

end loop

(9.14)

While Y defines the block-diagonal elements of Z, the following recursive expres-sions describe all the Z(i, j) terms, including the off-diagonal ones:

Z(i, j) =

⎧⎪⎪⎪⎨⎪⎪⎪⎩

Y(i) for i= j

A(i,k)Z(k, j) for i� k� j, k ∈ �(i)

Z(i,k)B∗(j,k) for i� k≺ j, k ∈ �(j)

0 otherwise

(9.15)

Proof: First let us verify that (9.12) is well-posed, i.e., that EAYE∗B

is block-diagonal when Y is block diagonal. This follows from:

EAYE∗B

8.37=

⎧⎨⎩n∑j=1

e℘(j)A(℘(j), j)e∗j

⎫⎬⎭Y

{n∑k=1

ekB∗(℘(k),k)e∗℘(k)

}

8.3=

n∑j=1

e℘(j)A(℘(j), j)Y(j)B∗(℘(j), j)e∗℘(j)

(9.16)

This has the form of a block-diagonal matrix with the kth diagonal element being∑j∈�(k) A(k, j)Y(j)B∗(k, j). This implies that, at the component level, (9.12) is

equivalent toX(k)

9.12,9.16= Y(k)−

∑∀j∈�(k)

A(k, j)Y(j)B∗(k, j)

from which (9.13) follows.Pre and post multiply (9.12) by A and B

∗ to get:

AXB∗ 8.19

= (A+ I)Y(B∗ + I)− AYB∗ = AY+YB

∗ +Y

This establishes (9.11).The middle pair of expressions in (9.15) follow directly from the structure of AY

and YB∗.

The disjoint terms in (9.11) represent the following:

• The non-zero block-elements of the block-diagonal Y are the block-diagonalelements of AXB

∗.• The non-zero block-elements of AY and YB

∗ correspond to related node pairs inthe system, i.e., nodes connected by a directed path.

168 9 SKO Models

• The remaining terms are zero, and correspond to unrelated node pairs, i.e., nodeswith no directed path connecting them.

Since entries corresponding to node pairs that do not have an ancestor/child re-lationship are zero, AXB

∗ is in general sparse. The sparsity structure reflects thetopological connectivity of the nodes. Based on (9.15), the strategy for computingAXB

∗ consists of:

1. First, compute the non-zero block-diagonal terms in Y using the tips-to-basegather algorithm in (9.14).

2. Now consider the case of computing Z(j,k), where j is an ancestor of k. Assumethat node i is the child of j on the path to k. Use (9.15) to compute Z(j,k) =A(j, i)Z(i,k). This expression implies a recursion that starts with the Z(k,k) =Y(k) diagonal element, and computes the Z(i,k) elements recursively along thepath to node j.

3. Now consider the converse case of computing Z(j,k), where j is a descendantof k. Assume that node i is the child of k on the path to j. Use (9.15) to computeZ(j,k) = Z(j, i)B∗(k, i). This expression implies a recursion that starts with theZ(j, j) = Y(j) diagonal element, and computes the Z(j, i) elements recursivelyalong the path to node k.

In the special case where A = B, case (3) above is unneeded becauseZ is symmetric,and Z(j,k) = Z∗(k, j).

9.3.2 Mass Matrix Computation for an SKO Model

As seen in (9.2), the mass matrix in the SKO model has the operator expressionM =HAMA

∗H∗. The following lemma uses Lemma 9.3 to derive a decompositionof, and computational procedure for, the mass matrix.

Lemma 9.4 SKO model mass matrix decomposition.The SKO model mass matrix can be decomposed into a sum of disjoint terms asfollows:

M =HRH∗ +HARH∗ +HRA∗H∗ (9.17)

where the block-diagonal composite body inertia operator, R, satisfies the followingforward Lyapunov equation:

M = R−EARE∗A (9.18)

Proof: The results here are a direct consequence of Lemma 9.3, once we identifyB = A, and X= M. From the lemma, we have the following disjoint decompositionof AMA

∗:AMA

∗ = R+ AR+RA∗ (9.19)


Equation (9.17) follows by pre and post multiplying the above expression with theblock-diagonalH and H∗, respectively.

The tips-to-base gather Algorithm 9.2 describes the general composite body in-ertia algorithm for an SKO model. This algorithm is a generalization of the serial-chain composite body inertia Algorithm 4.1 on page 60.

Algorithm 9.2 Recursive computation of composite body inertias for an SKOmodel ⎧⎪⎪⎪⎨

⎪⎪⎪⎩


R(k) =∑

∀i∈�(k)

A(k, i)R(i)A∗(k, i)+M(k)

end loop

(9.20)

Remark 8.2 on page 145 discussed the sparsity structure of the EA and A matricesfor the canonical tree in Fig. 8.2. Based on (9.17), the following matrix illustratesthe sparsity structure of the mass matrix for the same system:

M =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

X · X · · X X

· X · X · X X

X · X · · X X

· X · X · X X

· · · · X X X

X X X X X X X

X X X X X X X

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

The above matrix shows that, for trees, the M mass matrix contains block-elementsthat are zero, because, unlike serial-chains, trees can have unrelated nodes. Forserial-chain systems, the mass matrix is dense and fully populated because all nodesin a serial-chain system are related.

The sparsity of the mass matrix depends on the underlying branching structureof the system. As discussed in Sect. 14.3, the sparsity structure of the mass matrix isdetermined by the connectivity of its serial-chain segments; blocks corresponding tothe segments are dense, while those corresponding to unrelated segments are zero.Section 14.3 also examines the sparsity structure of the mass matrix for the morecomplex tree system in Fig. 14.3.

Based on the decomposition in (9.17), Algorithm 9.3 describes a recursive pro-cedure for computing the mass matrix of a tree-topology system. The outer loopincludes the tips-to-base gather computation of the composite body inertias. The in-ner loop computes the mass matrix cross-terms for all the ancestors of a body. Thisalgorithm for computing the mass matrix for SKO models is a generalization of thecorresponding serial-chain Algorithm 4.2 on page 64.

170 9 SKO Models

Algorithm 9.3 Recursive computation of the SKO model mass matrix⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩


R(k) =∑

∀i∈�(k)

A(k, i)R(i)A∗(k, i)+M(k)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

j= k, X(k) = R(k)H∗(k), M(k,k) =H(k)X(k)

while j

l= ℘(j)

X(l) = φ(l, j)X(j)

M(l,k) = M∗(k, l) =H(l)X(l)

j= l

end loop

end loop

9.3.3 Backward Lyapunov Recursions for SKO Models

Complementing the discussion on forward Lyapunov equations is the followinglemma regarding backward Lyapunov equations for SKO models.

Lemma 9.5 Structure of A∗XB.

Let A and B denote compatible SPO operators and X be a compatibleblock-diagonal operator. Then the Z = A

∗XB product can be expressed as thefollowing sum of disjoint terms:

Z= Y+ A∗Y+YB+R

where R�=

∑∀i,j: i⊀�jk=℘(i,j)

eiA∗(k, i)Y(k)B(k, j)e∗j (9.21)

Y is a block-diagonal operator satisfying the following backward Lyapunov equa-tion:

X= Y− diagOf{E∗

AYEB

}(9.22)

The diagOf{E∗

AYEB

}term represents just the diagonal block elements of the (gen-

erally non block-diagonal) E∗AYEB matrix. The Y(k) diagonal elements satisfy the

following parent/child recursive relationship:

Y(k) = A∗(℘(k),k)Y(℘(k))B(℘(k),k)+X(k) (9.23)


Based on this relationship, the Y(k) diagonal elements can be computed via thefollowingO(N) base-to-tips scatter recursion:

⎧⎪⎨⎪⎩


Y(k) = A∗(℘(k),k)Y(℘(k))B(℘(k),k)+X(k)

end loop

(9.24)

While Y defines the block-diagonal elements of Z, the following recursive expres-sions describe all the Z(i, j) terms, including the off-diagonal ones:

Z(i, j) =

⎧⎪⎪⎪⎨⎪⎪⎪⎩

Y(i) for i= j

A∗(k, i)Z(k, j) for i≺ k� j, k= ℘(i)

Z(i,k)B(k, j) for i� k� j, k= ℘(j)

A∗(k, i)Y(k)B(k, j) for i� j, j� i, k = ℘(i, j)

(9.25)

Proof: We have

E∗AYEB =

⎧⎨⎩n∑j=1

ejA∗(℘(j), j)e∗℘(j)

⎫⎬⎭Y

{n∑k=1

e℘(k)B(℘(k),k)e∗k

}

8.3=

∑∀j,k: ℘(j)=℘(k)

ejA∗(℘(j), j)Y(℘(j))B(℘(k),k)e∗k

Therefore,

diagOf{E∗

AYEB

}=

n∑j=1

ejA∗(℘(j), j)Y(℘(j))B(℘(j), j)e∗j (9.26)

This implies that, at the component level, (9.22) is equivalent to

X(k)9.22,9.26

= Y(k)−A∗(℘(j), j)Y(℘(j))B(℘(j), j)

from which (9.23) follows.Now let us examine the elements of Z= A

∗XB in (9.21).

Z=

⎧⎨⎩n∑i=1

∑∀m�i

eiA∗(m, i)e∗m

⎫⎬⎭X

⎧⎨⎩n∑j=1

∑∀l�j

elB(l, j)e∗j

⎫⎬⎭

=

n∑i=1

n∑j=1

∑∀m�i,j

eiA∗(m, i)X(m)B(m, j)e∗j

Hence,Z(i, j) =

∑∀m�i,j

A∗(m, i)X(m)B(m, j)

172 9 SKO Models

The expressions in (9.25) are a direct consequence of the above expression forZ(i, j).

Unlike AXB∗, A

∗XB is a fully populated matrix. The disjoint partitioned termsin (9.21) have the following properties:

• The block-diagonal Y contains the diagonal block elements terms of A∗XB.

• The non-zero block-elements of A∗Y and YB correspond to all related nodes.

• The non-zero block-elements of R on the other hand are for the remaining unre-lated node pairs, i.e., ones that have no directed path connecting them.

As is evident from (9.25), all the block-elements of A∗XB intimately dependent

on the elements of the Y block-diagonal matrix. An important application of thislemma is to operational space inertias, as discussed in Chap. 10. The recursive com-putational strategy for the elements of A

∗XB is described in Algorithm 9.4. Forthe special case where A = B, Z is symmetric, and Step (3) in the algorithm can beskipped since these elements can be obtained from the transposes of the correspond-ing elements in Step (2).

Case 1: =Z( , ) = Y( )

Case 2 & 3:Z( , ) = A

∗( , )Y( )Z( , ) = Y( )B( , )

Case 4: ,Z( , ) = A

∗( , )Y( )A( , )

i

iii

ii

jj

jj

j

jj

jj

jj

k

k

k

kk

k

kkk

kkk

k

kkk

k

Fig. 9.2 Illustration of the recursions for computing the Z(., .) elements for the three casesdescribed in Algorithm 9.4 for a tree. These recursions start from the Y(.) diagonal terms

The following Lemma describes simplifications of the backwards Lyapunovequation for serial-chain systems.

Lemma 9.6 A∗XB structure for a serial-chain SKO model.

For a serial-chain system, Z = A∗XB can be decomposed into disjoint diagonal,

strictly upper triangular, and strictly lower triangular terms as follows:

Z= Y+ A∗Y+YB (9.27)


Algorithm 9.4 Computation of Z= A∗XB

We have four situations to consider when computing a Z(k, j) element. Therespective recursive steps are described below and illustrated in Fig. 9.2.

1. To obtain a block-diagonal element, Z(k,k), compute the Y(k) element usingthe base-to-tips scatter O(N) algorithm described in (9.24) as illustrated in thediagram on the left in Fig. 9.2.

2. If the kth body is an ancestor of the jth body, Z(j,k) can be computed re-cursively, starting with the Z(k,k) ≡ Y(k) diagonal element, which, in turn,is computed using the process described in case (1). The recursion for Z(j,k),illustrated by the middle diagram of Fig. 9.2, starts with this Y(k) diagonal entry,and propagates it along the path from the kth to the jth body, using the secondexpression in (9.25) to compute the Z(k− 1,k), Z(k− 2,k), etc., terms.

3. If, on the other hand, the kth body is an ancestor of the jth body, then Z(k, j)can be computed recursively, starting with the Z(k,k) ≡ Y(k) diagonal element,which, in turn, is computed using the process described in case (1). The recursionfor Z(k, j), illustrated in the middle diagram of Fig. 9.2, starts with this Y(k)diagonal entry, and propagates it along the path from the kth to the jth body,using the third expression in (9.25) to compute the Z(k,k− 1), Z(k,k− 2), etc.,terms.

4. Now consider the remaining case where the (k, j) node pair are unrelated. In thiscase, identify the body that is the closest ancestor for this pair of bodies. If thebodies do not have a common ancestor, then Z(k, j) is zero since the bodiesbelong to independent, decoupled multibody trees.If, on the other hand, there is a common ancestor, denoted the ith body, thenthe first step is to compute Z(k, i), using the recursive procedure described incase (2). This value is then recursively propagated along the path from the ithbody to the jth body, using the procedure in case (3) to compute the Z(k, i− 1),Z(k, i− 2), etc., terms. This process is illustrated in the diagram on the right inFig. 9.2.

where Y ∈R6n×6n is a block-diagonal operator satisfying the following backwardLyapunov equation:

X= Y−E∗AYEB (9.28)

Assuming that the serial-chain is canonical, the symmetric positive semi-definiteY(k) diagonal matrices can be computed via the followingO(N) base-to-tip scatterrecursion:

⎧⎪⎪⎪⎨⎪⎪⎪⎩

Y(n+ 1) = 0

for k = 1 · · ·nY(k) = A

∗(k+ 1,k)Y(k+ 1)B(k+ 1,k)+X(k)

end loop

(9.29)

174 9 SKO Models

While Y defines the block-diagonal elements of Z, the following recursive expres-sions describe all the terms, including the off-diagonal ones:

Z(i, j) =

⎧⎪⎨⎪⎩Y(i) for i= j

A∗(i+ 1, i)Z(i+ 1, j) for i < j

Z(i, j+ 1)B(j+ 1, j) for i > j

(9.30)

Proof: Unlike trees, all nodes in a serial-chain systems are related, i.e., every pairof nodes is connected by a directed path. Hence, R = 0 in (9.21) for these systems.Substituting this in (9.21), and remembering that E∗

AYEB in (9.22) is block-diagonal

for serial-chains leads to this result.Observe that Step (4) in Algorithm 9.4 is not needed for serial-chain systems.

9.4 Riccati Equations for SKO Models

This section studies another type of SPO quadratic equation, known as a Riccatiequation, for SKO models.

Lemma 9.7 The Riccati equation for SKO models.Let (H,A,M) denote spatial operators for an SKO model. The following Riccatiequation has a block-diagonal, symmetric and positive-definite operator solu-tion, P:

M = P−EA

[

P−PH∗(HPH∗)−1HP]

E∗A

(9.31)

The expression in (9.31) can be broken down into simpler sub-expressions as fol-lows:

M = P−EA

[

P−PH∗(HPH∗︸︷︷︸D

)−1

︸︷︷︸G

H

︸︷︷︸τ

P

︸︷︷︸P+

]

E∗A

The above sub-expressions define the block-diagonal spatial operators:

D�= HPH∗, G

�= PH∗D−1, τ

�= GH

τ�= I−τ, P+ �

= τP = τPτ∗(9.32)

The P(k) and other diagonal elements can be computed by the following O(N)tips-to-base gather recursion:

9.4 Riccati Equations for SKO Models 175

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩


P(k) =∑

∀j∈�(k)

A(k, j)P+(j)A∗(k, j)+M(k)


G(k) = P(k)H∗(k)D−1(k)

τ(k) = G(k)H(k)

τ(k) = I−τ(k)

P+(k) = τ(k)P(k)

end loop

(9.33)

Proof: Equation (9.33) is essentially a component level restatement of (9.31).The positive definiteness of M and full row-rankness of H ensures that D−1(k)exists and is well defined.

This lemma is a generalization of the corresponding Riccati equation in Lemma7.1 on page 117 for serial-chain rigid body systems.

9.4.1 The Eψ and ψ SKO and SPO Operators

Lemma 9.8 The Eψ SKO operator.Let EA be an SKO operator, and H, M, be compatible operators satisfying theconditions in Lemma 9.7. Let P denote the block-diagonal solution to the (9.31)Riccati equation. Define

Eψ�= EAτ (9.34)

Then Eψ is an SKO operator, with weights and SPO operatorψ, defined by

ψ(℘(k),k)�= A(℘(k),k)τ(k), and ψ

�= (I−Eψ)−1 (9.35)

Moreover, the (9.31) Riccati equation can be re-expressed as:

M = P−EψPE∗ψ = P−EAPE∗

ψ (9.36)

Proof: Since EA is an SKO operator, we have

Eψ9.34=

(

n∑k=1

e℘(k)A(℘(k),k)e∗k

)

τ8.22=

n∑k=1

e℘(k)A(℘(k),k)τ(k)e∗k

9.35=

n∑k=1

e℘(k)ψ(℘(k),k)e∗k

176 9 SKO Models

This implies that Eψ is indeed an SKO operator, with weights defined byψ(℘(k),k).Thus, its 1-resolvent,ψ, defined by (9.35) exists.The first part of the Riccati equation expression in (9.36). This is equivalent to (9.31)follows from the direct use of the expressions in (9.32) to observe that:

P−PH∗(HPH∗)−1HP = τP = Pτ∗ = τPτ∗ = P+

Unlike the EA SKO operators, which are defined during the SKO model formula-tion process, the Eψ SKO operators are derived operators obtained from the solutionto the Riccati equation. Likewise, the ψ SPO operator is also a derived by-productof the solution of the Riccati equation. Observe that the ψ(℘(k),k) matrices aresingular even when A(℘(k),k) is non-singular.

9.4.2 Operator Identities

Define the K spatial operator as follows:

K = EAG (9.37)

K is not block-diagonal, but has structure similar to that of EA. The following lemmaestablishes operator identities that will be needed later.

Lemma 9.9 Useful spatial operator identities.

1.HG = I, Hτ=H, Hτ= 0

I+HAK =HAG(9.38)

2.HAτ=HAEψ (9.39)

3.HAτ=HA(I−Eψ) =HAψ−1 (9.40)

4. AMψ∗ has the following disjoint decomposition:

AMψ∗ = P+ AP+Pψ∗ (9.41)

Proof:

1. The expressions in (9.38) follow from (9.32) and (9.37).2. We have

HAτ8.19= H(A+ I)τ

8.19= HAEAτ+Hτ

9.34,9.32= HAEψ

9.4 Riccati Equations for SKO Models 177

3. We have

HAτ9.32= HA(I−τ)

9.39= HA(I−Eψ)

9.35= HAψ−1

4. Pre and post multiplying (9.36) by A and ψ∗, then simplifying, leads to (9.41).

The following lemma derives additional operator identities. Several of these iden-tities (and their proofs) are similar to those in Lemma 7.2 on page 119 for serial-chain systems, but, for completeness, they are repeated here in the more generalSKO model context.

Lemma 9.10 Additional spatial operator identities.

1.ψMψ∗ = P+ ψP+Pψ∗ (9.42)

2.ψ−1 −A

−1 = KH (9.43)

3.ψ−1

A = I+KHA

Aψ−1 = I+AKH

A−1ψ= I−KHψ

ψA−1 = I−ψKH

(9.44)

4.[I−HψK]HA =Hψ

AK[I−HψK] =ψK

[I+HAK]Hψ=HA

ψK[I+HAK] = AK

(9.45)

5.HψMψ∗H∗ = D (9.46)

6.AMA

∗ = P+ AP+PA∗+AKDK∗

A∗ (9.47)

7.AMA

∗H∗ = [I+AKH]PA∗H∗ (9.48)

Proof:

1. Pre- and post-multiplying (9.36) by ψ and ψ∗, we obtain

ψMψ∗ =ψPψ∗ −ψEψPE∗ψψ

∗ 8.19= (ψ+ I)P(ψ+ I)∗ − ψPψ∗

= P+ ψP+Pψ∗

178 9 SKO Models

2. From (8.15)

ψ−1 = I−Eψ9.32= I−EAτ

9.32= (I−EA)+EAτ

8.15,9.32= A

−1 +EAGH9.32= A

−1 +KH

3. Pre- and post-multiplying (9.43) by A leads to the first pair of identities in (9.44).Repeating the process using ψ leads to the latter pair.

4. We have

[I−HψK]HA =H[I−ψKH]A9.44= H(ψA

−1)A =Hψ

Similar use of the other identities in (9.44) leads to the remaining identities in(9.45).

5. We have

HψMψ∗H∗ 9.36= H(P+ ψP+Pψ∗)H∗

9.32= D+HψPH∗ +HPψ∗H∗

8.19= D+HψEψPH∗ +HPE∗

ψψ∗H∗

9.32= D+HψEAτPH

∗ +HPτ∗E∗Aψ

∗H∗

9.32= D+HψEAP+H∗ +HP+E∗

Aψ∗H∗

9.38,9.32= D

6. Now

AMA∗ = (Aψ−1)ψMA

∗ 9.41= (Aψ−1)(ψP+PA

∗)9.43= AP+(I+AKH)PA

∗ = AP+PA∗ +AKHPA

∗

9.32,9.37,8.19= P+ AP+PA

∗+AKDK∗A∗

7. We have

AMA∗H∗ = (Aψ−1)(ψMA

∗)H∗ 9.41= (Aψ−1)(ψP+PA

∗)H∗

8.19= AEψPH∗ +(Aψ−1)PA

∗H∗

9.32= AEψP+H∗ +(I+AKH)PA

∗H∗

9.38= (I+AKH)PA

∗H∗

It is noteworthy that the HψMψ∗H∗ expression in (9.46) is block-diagonal, incontrast to the similar expression, M = HAMA

∗H∗, for the non-diagonal massmatrix.

9.5 SKO Model Mass Matrix Factorization and Inversion 179

9.5 SKO Model Mass Matrix Factorization and Inversion

The following lemma derives the mass matrix factorization and inversion propertiesfor SKO models.

Lemma 9.11 SKO mass matrix factorization and inversion.Let (H,A,M) denote spatial operators for an SKO model. Recall that its mass-matrix is defined as M =HAMA

∗H∗.

1. The M mass matrix has an alternative Innovations Operator factorizationdefined by

M = [I+HAK]D [I+HAK]∗ (9.49)

In this equation, D and K are operators obtained from the solution of the discreteRiccati equation from Lemma 9.8. for the SKO model.

2. [I+HAK] is invertible, with inverse given by:

[I+HAK]−1 = [I−HψK] (9.50)

3. The mass matrix M is invertible, and the expression for its inverse is

M−1 = [I−HψK]∗D−1 [I−HψK] (9.51)

Proof:

1. We have

M =HAMA∗H∗ =H(Aψ−1)ψMψ∗(Aψ−1)∗H∗

9.44= H[I+AKH]ψMψ∗[I+AKH]∗H∗

= [I+HAK](HψMψ∗H∗)[I+HAK]∗

9.46= [I+HAK]D[I+HAK]∗

2. From the standard matrix identity (I+AB)−1 = I−A(I+BA)−1B, we have:

[I+HAK]−1 = I−H[I+AKH]−1AK

9.44= I−H(Aψ−1)−1

AK

= I−HψK

3. We have

M−1 9.49=

{[I+HAK]D[I+HAK]∗

}−1= [I+HAK]−∗D−1[I+HAK]−1

9.50= [I−HψK]∗D−1[I−HψK]−1

This lemma is a generalization of the serial-chain, rigid multibody system massmatrix factorization and inversion results from Chap. 7.

180 9 SKO Models

Exercise 9.2 Determinant of the mass matrix.

1. Show that [I+HAK] and [I−HψK] are strictly lower triangular for canonicaltrees, and that they have identity block matrices along the diagonal.

2. Show that the determinant of the Newton–Euler operator factor [I+HAK] is

det {I+HAK} = 1 (9.52)

3. Furthermore, show that the determinant of the mass matrix is given by

det {M} =

n∏k=1

det {D(k)} (9.53)

9.5.1 O(N) AB Forward Dynamics

The forward dynamics problem consists of computing the θ generalized accelera-tions, given the T generalized forces for the system. The following lemma derivesan explicit operator expression for θ. This lemma is a generalization of the corre-sponding Lemma 7.6 for canonical serial-chain systems.

Lemma 9.12 Expression for θ= M−1(T −C).The explicit expression for θ is

θ = M−1(T −C) = [I−HψK]∗D−1[T −Hψ(KT +Pa+b)]

−K∗ψ∗a (9.54)

Proof: We have

θ9.51= [I−HψK]∗D−1[I−HψK](T −C)

9.2= [I−HψK]∗D−1[I−HψK](T −HA(MA

∗a+b))

= [I−HψK]∗D−1[I−HψK]T

−[I−HψK]∗D−1[I−HψK]HA(MA∗a+b)) (9.55)

Now,

9.5 SKO Model Mass Matrix Factorization and Inversion 181

[I−HψK]HA(MA∗a+b) =H[I−ψKH]A(MA

∗a+b)

9.44= H(ψA

−1)A(MA∗a+b)

=Hψ(MA∗a+b)

9.41= H

(

(ψP+PA∗)a+ψb

)

9.32= (HψP+DK∗

A∗)a+Hψb (9.56)

Substituting this expression into the second half of (9.55), it follows that

[I−HψK]∗D−1[I−HψK]HA(MA∗a+b))

= [I−HψK]∗D−1[(HψP+DK∗A∗)a+Hψb

]

9.44= [I−HψK]∗D−1[HψPa+Hψb]+K∗(A−1ψ)∗A

∗a9.44= [I−HψK]∗D−1Hψ(Pa+b)+K∗ψ∗a

(9.57)

Substituting this expression in (9.55) leads to (9.54).Using similar steps to those in Sect. 7.2.1 on page 123, we group together sub-

expressions in (9.54) to define intermediate quantities as shown below:

θ = [I−HψK]∗D−1[T −Hψ(KT +Pa+b)︸︷︷︸z︸︷︷︸

ε

]

︸︷︷︸ν

−K∗ψ∗a

Using these intermediate quantities, and simplifications similar to those in Sect.7.2.1, (9.54) can be re-expressed as

z = Eφz+ +Pa+b (9.58a)

z+ = z+Gε (9.58b)

ε = T −Hz (9.58c)

ν = D−1ε (9.58d)

α+ = E∗φα (9.58e)

θ = ν−G∗α+ (9.58f)

α = α+ +H∗θ+a (9.58g)

Recall that ψ is an SPO operator. We can thus use Lemmas 9.1 and 9.2 to developa recursive algorithm for evaluating θ from the expressions in (9.58). The resultingO(N) AB forward dynamics algorithm for SKO models is described in Algorithm9.5. It is a generalization of Algorithm 7.1 on page 126 for the AB forward dynamicsof serial-chain rigid body systems. The algorithm consists of a tips-to-base gathersweep to compute the residual terms, followed by a base-to-tips scatter sweep forthe generalized accelerations.

182 9 SKO Models

Algorithm 9.5 O(N) AB forward dynamics for SKO models

1. Compute the articulated body inertia P, D, etc., quantities using the gather algo-rithm in (9.33).

2. Use the following gather and scatter recursions to compute θ:⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩


z(k) =∑

∀j∈�(k)

A(k, j)z+(j)+P(k)a(k)+b(k)


ν(k) = D−1(k)ε(k)

z+(k) = z(k)+G(k)ε(k)

end loop

⎧⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎩


α+(k) = A∗(℘(k),k)α(℘(k))

θ(k) = ν(k)−G∗(k)α+(k)

α(k) = α+(k)+H∗(k)θ(k)+a(k)

end loop

9.6 Generalized SKO Formulation Process

In this chapter, we have derived analytical techniques and efficient algorithms forSKO models of multibody systems. These have included

• Recursive O(N) procedures for computing SPO operator and stacked vectorproducts.

• GeneralO(N) Newton–Euler inverse dynamics algorithms.• Solutions for the forward Lyapunov equations and decomposition of AXB

∗ op-erator product.

• GeneralO(N2) algorithm for computing the mass matrix.• Solution for the backward Lyapunov equations and decomposition of A

∗XB op-erator product.

• Recursive algorithms for computing the A∗XB operator product.

• The generalO(N) articulated body inertia solution of the Riccati equation.• Several operator identities.• The alternative Innovations Operator Factorization of the mass matrix.• An analytical expression for the inverse of the mass matrix.• The analytical expression for the determinant of the mass matrix.• The generalO(N) AB forward dynamics algorithm.

9.6 Generalized SKO Formulation Process 183

The above results required no assumptions on the SKO model regarding the SKOweight matrices, the components of the other spatial operators, or the structure of thetree digraph. Thus, any multibody system formulation satisfying the requirements ofthe SKO model has available to it the full spectrum of these techniques and efficientalgorithms. With this as motivation, we outline the general steps in the developmentof an SKO multibody dynamics formulation:

Develop an SKO model: The key starting point of an SKO formulation is thedevelopment of an SKO model for the system. A systematic procedure for this isdeferred till Sect. 9.6.1.

Apply SKO techniques: Once the SKO model has been developed, the analyti-cal results and efficient algorithms for spatial operators described above can beapplied to the system.

Optimize algorithms: Finally, optimizations that take advantage of the specificfeatures of the SKO model can be further applied to the SKO algorithms. Thoughthe SKO algorithms are already highly efficient, there are usually several oppor-tunities for further optimizations based on the specific structure, sparsity, andredundancies of the SKO weight matrices, joint map matrices, etc. Even thoughsuch system specific optimization are applied only in this last step, they can sig-nificantly transform the structure of the eventual algorithms.

9.6.1 Procedure for Developing an SKO Model

Outlined below is a procedure for developing an SKO model for general multibodysystems. It provides guidelines for identifying the SKO weight matrices and thecomponents of the H and M operators. Figure 9.3 illustrates the key steps in theprocedure.

1. Identify system tree digraph: First, identify a tree digraph for the SKOmodel. For tree-topology multibody systems, this is usually straightforward,with the standard tree digraph for the system being a good candidate.However, there is no requirement that the nodes in the tree digraph be in one-to-one correspondence with the physical bodies in the system, as is the case for thestandard tree digraph. Chapter 14 describes partitioning and aggregation tech-niques for tree digraphs that allow the assignment of multiple bodies to singlenodes in the SKO model tree.For non-tree systems, Chap. 15 discusses techniques to transform the non-tree di-graph into a tree digraph using constraint embedding techniques. Also, Chap. 11describes the classical augmented constrained dynamics, which uses the span-ning tree as the tree digraph for the critical parts of the forward dynamics solutionprocedure.

2. Node equations of motion: Establish the equations of motion of the compo-nent nodes in the system. To accomplish this, identify the V(k) velocities for thecomponent nodes. The size of the velocity vector for a node determines the mk

184 9 SKO Models

2.

.

.

Rigid-link, tree topology systemprocedure

General procedure

Node equations of motion

Identify inter-node velocityrelationship

Assemble Newton-Euler massmatrix & equations of motion

V(k) body spatial velocityM(k) body spatial inertiaf(k) = M(k)α(k)+b(k)

φ(℘(k),k) SKO weight matricesH(k) joint map matricesθ(k) hinge generalized velocitiesV(k) = φ∗(℘(k),k)V(℘(k))+H∗(k)θ(k)

M = HφMφ∗H∗C = Hφ(Mφ∗a+b)

3.

4.

Fig. 9.3 Flow chart illustrating steps 2 through 4 of the process described in Sect. 9.6.1for developing SKO models for multibody systems. The corresponding steps for rigid-link,tree-topology multibody systems are shown on the right

BWA weight dimension for the node. Also, identify the appropriatemk×mk di-mensional M(k) inertia matrix for each node. Together with the node velocity,the 1

2V∗(k)M(k)V(k) expression should define the kinetic energy contributionof the node.

3. Identify inter-node velocity relationships: Identify the recursive relationshipbetween the V(k) velocity of a node and and that of its parent node, and the θ(k)generalized velocities of the hinge connecting them. This will help identify theparent/child SKO weight matrix, A(℘(k),k), and the componentH(k) joint mapmatrices associated with the connecting hinge. The A

∗(℘(k),k) term defines thecontribution of the V(℘(k)) parent node’s velocity to V(k) while H∗(k) definesthe contribution from the θ(k) generalized velocities.The hinges connecting adjacent nodes are not required to be physical hinges.This is especially true in cases where multiple bodies are assigned to nodes inthe digraph.

4. Assemble Newton–Euler mass matrix expression: Assemble the system-level stacked vectors for the hinge generalized velocities, the node velocities,etc., and the various SKO, SPO, etc., spatial operators leading to the system-levelequations of motion, the Newton–Euler factorization of the M mass matrix andthe C nonlinear Coriolis and velocity terms vector in (9.1) through (9.3).

9.6 Generalized SKO Formulation Process 185

In later chapters we will see several examples illustrating the use of this procedure.In particular, Sect. 13.3.2 on page 257 discusses, in detail, its application to multi-body systems with deformable links.

9.6.2 Potential Non-Tree Topology Generalizations

The digraph in an SKO model is required to be a directed tree. We address herethe potential extension of an SKO model to work with non-tree digraphs. First, werecall key steps in the SKO model development:

1. Set up of the implicit (8.36) relationship V= E∗AV+H∗ θ for the V node velocities

using independent generalized hinge velocity coordinates.2. Use the nilpotency of the EA BWA matrix to derive its A 1-resolvent to convert

this implicit expression into the V = A∗H∗ θ explicit form in (8.38).

Non-tree digraphs can contain directed cycles as well as multiply-connected nodes.We now examine specific issues in developing SKO models for such non-tree di-graph systems.

Digraphs with directed cycles: For digraphs with directed cycles, the BWA ma-trices are not nilpotent, and their 1-resolvents do not exist. Intuitively, the pres-ence of cycles implies that there are paths of arbitrary lengths connecting nodesthat are part of such cycles, since paths that loop around the cycle multiple timesare legal. Thus, by Lemma 8.1, none of the powers of the BWA matrix, with di-rected cycles, are non-zero. Hence, their 1-resolvent SPO operators do not exist.This means that step (2) above breaks down, and the implicit relationship for V

in (8.36) cannot be transformed into the explicit (8.38) form.Multiply-connected DAGs: Multiply-connected DAGs are digraphs containing

a node with more than one parent. For these systems, the BWA matrix is stillnilpotent and, hence, its 1-resolvent is well defined. The catch for such systemsis the inability to identify independent generalized hinge velocity coordinates forthe edges. Such relationships are unique and well-defined when bodies have sin-gle parents. However, when a body has multiple parents, the parent/child nodevelocity relationship must hold simultaneously for each parent. For them to holdsimultaneously, the pair of generalized hinge velocity coordinates must be mu-tually consistent and, hence, they are not independent. Step (1) above thereforebreaks down.

Thus, we see that the use of non-tree digraphs with SKO models has potential prob-lems. This complicates the formulation of the dynamics of closed-chain systems.Later chapters discuss avenues for overcoming these hurdles in order to developand use SKO models for closed-chain systems.

Chapter 10Operational Space Dynamics

Robotic platforms are typically designed to interact with the environment at specifictask space nodes on the system. For a manipulator, the set of task space nodes mayconsist of a single end-effector node. For a humanoid system, the arm end-effectorsare task space nodes, while the feet are such nodes for legged systems. The set oftask space nodes define the operational space of the system. The control problemrequires managing the motion state as well as the force interactions with the envi-ronment for the task space nodes. Operational space control (OSC) is an approachto robot system control that focuses on the dynamical behavior of a system fromthe task (or operational) space perspective [100–103]. It is especially useful for ap-plications involving contact between the end-effector and the environment, such asoccurs in hybrid force/position control, or for artificial potential field approachesto collision avoidance and path planning. The advantage of the OSC approach overjoint space control is that the control problem is posed directly in terms of taskspace variables. One issue is the added analytical and computational complexity ofoperational space dynamics. As will be seen in later chapters, the importance ofoperational space dynamics quantities extends to the dynamics of systems subjectto closure constraints, as well as to the dynamics of under-actuated and free-flyingsystems. In this chapter, we study and analyze operational space dynamics and as-sociated efficient computational algorithms for a robotic system.

While the only fundamental requirement is that the multibody system have anSKO model, for the sake of exposition, we will use the operator notation for tree-topology, rigid multi-link systems in this chapter. There is no loss in generality sincethe ideas developed here easily extend to general SKO models using the approachfrom Chap. 9.

10.1 Operational Space Equations of Motion

The operational (or task) space of a system is defined by the configuration of a set ofdistinguished nodes on the system. We adopt the notation from Sect. 3.6 on page 53


188 10 Operational Space Dynamics

and denote the number of task space nodes by nnd. Vnd ∈ R6nnd is the stackedvector of spatial velocities of all the nnd task space nodes. The relationship betweenVnd and the θ joint velocities is given by

Vnd3.50= B∗V 3.52

= B∗φ∗H∗ θ 3.53= Jθ (10.1)

J denotes the Jacobian for the task space nodes defined in (3.53). The task spacespatial acceleration αnd ∈ R6nnd is defined as the time derivative of Vnd and,from (10.1), its expression is given by

αnd�=

dVnd

dt10.1= B∗α+ B

∗V

9.1= B∗φ∗H∗ θ+aos

3.53= Jθ+aos (10.2)

whereaos

�= Jθ

5.26= B∗φ∗a+ B

∗V (10.3)

We have left unspecified the time derivative frame in (10.2). As discussed inSect. 5.1.3 on page 83, while specific choice of derivative frames can vary, the es-sential requirement is that the choice be consistent with the derivative frames usedin the definition of α, a etc. For the sake of generality, we will continue to avoidspecifying such derivative frames.

In the operational space perspective, the system experiences the fnd spatialforces at the operational space nodes. Recall from Remark 5.2.1 on page 85 thatthe joint space equations of motion for the system have the form

Mθ+C−J∗fnd5.36= T ⇒ θ = M−1 [T −C+J∗fnd] (10.4)

Pre-multiplying (10.4) by J and using (10.2) yields:

αnd10.2,10.4

= JM−1 [T −C+J∗fnd]+aos

=Λfnd+{JM−1 (T −C)+aos

} (10.5)

whereΛ

�= JM−1J∗ ∈R6nnd×6nnd (10.6)

When Λ is invertible, (10.5) can be re-expressed as

Λαnd+Cos = fnd where Λ�= Λ−1 ∈R6nnd×6nnd (10.7)

withCos

�= −Λ

{JM−1 (T −C)+aos

}(10.8)

Equation (10.7) defines the operational space equations of motion. It establishes the

relationship between the fnd operational space spatial forces and the αnd spatialacceleration of the nodes. The quantity, Λ, defined in (10.7) is referred to as theoperational space inertia of the system for its task space nodes, while its inverse

10.1 Operational Space Equations of Motion 189

Λ is referred to as the operational space compliance matrix. The invertibility ofΛ does not depend on J being invertible – only that it be full-rank.

Observe that the Vnd = Jθ relationship from (10.1) represents a, potentially non-invertible, transformation of the generalized velocity coordinates for the operationalspace formulation. When J is indeed square and invertible, from (10.6) and (10.7)we have Λ= J−∗MJ−1 which agrees with the mass matrix expression in (4.50) onpage 72 for invertible transformations of generalized velocity coordinates.

10.1.1 Physical Interpretation

The operational space inertia, Λ, represents the system mass matrix reflected tothe operational space nodes. Cos contains the corresponding velocity dependentCoriolis and gyroscopic terms. Though our primary interest is in Λ, we will spendmuch of our attention studying theΛ compliance matrix because the latter is alwayswell-defined and the path to obtainingΛ typically requires computingΛ. It is readilyobvious that, while Λ is always well defined, its inverse, Λ, is undefined when theJ Jacobian matrix is singular, i.e., when the system is in a kinematically singularconfiguration. In this respect, the Λ operational space inertia differs from the M

joint space mass matrix that is always well-defined and invertible for tree systems.Physically, the 6×6 Λ(Oik,Oik) block-diagonal elements of Λ define the effec-

tive “inertia” of the system at the Oik node, i.e., the mapping between an external

spatial force applied at the node and the induced spatial acceleration at the node.An off-diagonal ×6 Λ(Oik,Olj) block element is a measure of the cross-coupling

between the Oik and O

lj nodes. It represents the effective “inertia” coupling between

an external spatial force applied at the Oik node and the spatial acceleration induced

at the Olj node. For a manipulator with a single end-effector task space node, the

operational space inertia is a measure of the effective system inertia as seen at theend-effector node.

10.1.2 Operational Space Control

In the OSC approach to control, the feedback linearization strategy is to choose fndto be

fnd =Λu+Cos (10.9)

where u defines the desired spatial acceleration of the task space nodes. Assumingthat Λ is non-singular, this transforms (10.7) into the following form:

αnd = u (10.10)


The decoupled and linear transformed dynamics represent a much simpler controlproblem. When the dimension of the operational space generalized coordinates,Vnd, is less than the dimension of joint space coordinates, N, we have redundantdegrees of freedom in the system that can be used to meet additional task objectives.

One challenge of the OSC approach is the need for the explicit evaluation of theΛ operational space inertia and the Cos Coriolis/centrifugal term needed by (10.9).With Λ = (JM−1J∗)−1, its computation ostensibly requires the explicit computa-tion of Jacobian and the mass matrix inverse, a computation of their products, andthe eventual inversion of the resulting matrix. These operations are of O(N3) cost,and are computationally expensive for real-time control applications.

10.2 Structure of the Operational Space Inertia

We now begin examining the structural properties of the operational space inertiaand related quantities and the development of efficient algorithms for their compu-tation.

10.2.1 The Ω Extended Operational Space Compliance Matrix

Recalling the J = B∗φ∗H∗ Jacobian matrix expression from (3.53), leads to thefollowing expression for Λ:

Λ10.6= JM−1J∗ 3.53

= B∗φ∗H∗(I−HψK)∗D−1(I−HψK)HφB (10.11)

The use of the (I−HψK)Hφ=Hψ identity from (9.45) on page 177 results in thefollowing simpler expression:

Λ= B∗ΩB, where Ω�= ψ∗H∗D−1Hψ ∈R6n×6n (10.12)

We refer to Ω as the extended operational space compliance matrix. This ter-minology comes from (10.12) which shows that the operational space compliancematrix, Λ is obtained by a reducing transformation of the full, all body Ω matrixby the B pick-off operator to just the task space nodes. From its definition, it isclear thatΩ is a symmetric, positive semi-definite matrix, since D−1 is a symmetricpositive-definite matrix. From the expressions forΛ andΩ in (10.12), it follows thatthe explicit computation of M−1 or J is not needed to obtain Λ. While this reducesthe computational cost, the direct evaluation of (10.12) still remains O(N3) due tothe need for carrying out the matrix products.

10.2 Structure of the Operational Space Inertia 191

Physically, each diagonal Ω(k,k) block element of Ω defines the effective“compliance” at the kth body, i.e., the mapping between an external spatial forceapplied at the Bk body frame of the kth body and the induced spatial accelerationat the frame. On the other hand, an off-diagonalΩ(k, j) block element is a measureof the cross-coupling between the kth and jth bodies and represents the effective“compliance” mapping between an external spatial force applied at the Bj bodyframe of the jth body and the spatial acceleration induced at the Bk frame of thekth body.

10.2.2 Decomposition ofΩ

Since H∗D−1H is block-diagonal, and ψ is an SPO operator, the ψ∗H∗D−1Hψ

expression forΩ meets the conditions for the backwards Lyapunov recursion baseddecomposition in Lemma 9.5 on page 170. The following lemma uses it to derive adecomposition of Ω into simpler component terms and an expression for its blockelements.

Lemma 10.1 Decomposition of Ω.Ω can be decomposed into the following disjoint sum of component terms:

Ω = Υ+ ψ∗Υ+Υψ+R

where R�=

∑∀i,j: i⊀�jk=℘(i,j)

eiψ∗(k, i)Y(k)ψ(k, j)e∗j (10.13)

Υ ∈ R6n×6n is a block-diagonal operator, referred to as the operational spacecompliance kernel, satisfying the following backward Lyapunov equation:

H∗D−1H = Υ− diagOf{E∗ψΥEψ

}(10.14)

diagOf{E∗ψΥEψ

}represents just the block-diagonal part of the (generally non

block-diagonal) E∗ψΥEψ matrix. The 6× 6 dimensional, symmetric, positive semi-

definite Υ(k) diagonal matrices satisfy the following parent/child recursive rela-tionship:

Υ(k) =ψ∗(℘(k),k)Υ(℘(k))ψ(℘(k),k)+H∗(k)D−1(k)H(k) (10.15)

This relationship forms the basis for the following O(N) base-to-tips scatter recur-sion for computing the Υ(k) diagonal elements:

⎧⎪⎨⎪⎩


Υ(k) =ψ∗(℘(k),k)Υ(℘(k))ψ(℘(k),k)+H∗(k)D−1(k)H(k)

end loop

(10.16)


Algorithm 10.1 Computation of theΩ(k, j) elementsThis algorithm is based on Algorithm 9.4 on page 173, with simplifications arisingbecause A = B = ψ, and the symmetry of Ω. This algorithm drops case (2) fromAlgorithm 9.4 and uses the transposes of the elements computed from case (3)instead. Thus, we have three different situations to consider when computing aΩ(k, j) element. The respective recursive steps are described below and illustratedin Fig. 10.1.

1. To obtain a block-diagonal element, Ω(k,k), the procedure is to compute theΥ(k) operational space compliance kernel element using the base-to-tips scatterO(N) procedure in (10.16) or the more optimized algorithm in Algorithm 10.3described later.

2. If the kth body is an ancestor of the jth body, then Ω(k, j) can be computedrecursively starting with the Ω(k,k) ≡ Υ(k) diagonal element, which in turn iscomputed using the process described in case (1). The recursion forΩ(k, j) startswith this diagonal entry, and propagates it along the path from the kth to thejth body. The second expression in (10.17) is used to compute the Ω(k,k− 1),Ω(k,k− 2), etc., terms.If, on the other hand, the jth body happens to be the ancestor of the kth body,then compute Ω(j,k) using the above process, and take its transpose to obtainΩ(k, j).

3. Now consider the remaining case where the (k, j) pair of bodies are unrelated,i.e., neither is the ancestor of the other. In this case, identify the body that isthe closest ancestor for this pair of bodies. If the bodies do not have a commonancestor, then Ω(k, j) is zero since the bodies belong to independent, decoupledmultibody trees.If, on the other hand, there is a common ancestor, denoted the ith body, then thefirst step is to compute Ω(k, i) using the recursive procedure described in case(2). This value is then recursively propagated along the path from the ith bodyto the jth body, using the last expression in (10.17) to compute the Ω(k, i− 1),Ω(k, i− 2), etc., terms.

While Υ defines the block-diagonal elements of Ω, the following recursive ex-pressions describe the off-diagonal terms as well:

Ω(i, j) =

⎧⎪⎪⎪⎨⎪⎪⎪⎩

Υ(i) for i = j

Ω(i,k)ψ(k, j) for i� k� j, k= ℘(j)

Ω∗(j, i) for i≺ jΩ(i,k)ψ(k, j) for i� j, j� i, k= ℘(i, j)

(10.17)

Proof: This lemma is a direct application of Lemma 9.5 on page 170 with EA =EB = Eψ, and A = B = ψ, and the block-diagonalX=H∗D−1H.

This lemma shows thatΩ can be decomposed into the sum of simpler terms. Fur-thermore, (10.17) reveals that all of the block-elements of Ω(i, j) can be obtained


Case 1: =Ω( , ) = Υ( )

Case 2:Ω( , ) = Υ( )ψ( , )

Case 3: ,Ω( , ) = ψ∗( , )Υ( )ψ( , )

i

iii

ii

jj

jj

j

jj

jj

k

k

k

kk

k

kkk

k

kkk

k

Fig. 10.1 Illustration of the recursions for computing the Ω(k,j) elements for the threecases described in Algorithm 10.1 for a tree. These recursions start from the Υ(.) diagonalterms

from the Υ(i) elements of the Υ block-diagonal operational space compliance ker-nel. Since only a small subset of the elements of Ω are needed for computing Λ,Sect. 10.2.3 exploits this to avoid the expensive computation of the fullΩ matrix.

As pointed out in the discussion following Lemma 9.5,Ω is fully populated andhas no non-zero entries for a tree-system. This is because non-zero spatial forcesapplied at a body induce non-zero accelerations in every other body in the system.Zero entries arise only for body pairs where the bodies are on independent, disjointtrees within the multibody system.

Algorithm 10.1 uses the decomposition in (10.13) to derive a procedure for com-puting the required Ω(k, j) elements. The cost of computing any such element isat most O(N), and many of the intermediate values can be reused when additionalelements are needed.

10.2.3 Computing Λ

From the Λ= B∗ΩB expression, and the sparse structure of B, it is clear that onlya subset of the elements of Ω are needed to compute Λ. As discussed in Sect. 3.6,the B pick-off operator has one column for each of the O

ik task space nodes, with

each such column having only a single non-zero 6×6 matrix entry at the kth parentlink slot. From this structure of B and (10.12), the 6× 6 block matrix elements ofthe Λ operational space compliance matrix are given by the following expression:

Λ(Oik,Olj) = φ∗(k,Oik)Ω(k, j)φ(j,Olj) (10.18)


Here, Oik and O

lj denote a pair of task space nodes on the kth and jth links, respec-

tively. It is therefore evident that only as many elements ofΩ as there are elements inΛ are needed. Thus, just nnd×nnd 6×6 block-entries ofΩ are required. In viewof the symmetry of the matrices, even this reduces to needing just nnd(nnd+1)/2entries. The complete algorithm for the computation of the Λ operational space in-ertia is summarized by Algorithm 10.2.

Algorithm 10.2 Computation of the Λ operational space inertia

1. Use (10.18), together with Algorithm 10.1, to compute all the Λ(i, j) block ele-ments of Λ.

2. ComputeΛ=Λ−1 by numerical matrix inversion.

As observed in the previous section, the Ω extended operational space compli-ance matrix depends greatly on the Υ operational space compliance kernel operator.The next section looks at the properties of Υ and develops an efficient O(N) algo-rithm for computing its elements. This algorithm is a generalization of the one in[73, 152] and is the lowest order algorithm available. Other algorithms for comput-ing Λ are described in [34, 52].

10.2.4 The Υ Operational Space Compliance Kernel

Using ψ(℘(k)) = φ(℘(k))τ(k) from (9.35), (10.16) can be rearranged as follows:

Υ+(k)�= φ∗(℘(k),k)Υ(℘(k))φ(℘(k),k)

Υ(k)�= τ∗(k)Υ+(k)τ(k)+H∗(k)D−1(k)H(k)

and Υ+ �= diag

{Υ+(k)

}nk=1

∈R6n×6n

(10.19)

The block-diagonal Υ+ represents just the block-diagonal part of (the generallynon block-diagonal) E∗

φΥEφ, i.e.,

Υ+ = diagOf{E∗φΥEφ

}(10.20)

Recalling that τ(k) = I−τ(k) = I−G(k)H(k), from (9.33), the expression forΥ(k)in (10.19) can be simplified as follows:

Υ(k)10.19= τ∗(k)Υ+(k)τ(k)+H∗(k)D−1(k)H(k)

9.32= Υ+(k)−Υ+(k)τ(k)−τ∗(k)Υ+(k)+τ∗(k)Υ+(k)τ(k)

+H∗(k)D−1(k)H(k)


9.32= Υ+(k)−Υ+(k)G(k)︸︷︷︸

Z(k)

H(k)−H∗(k)G∗(k)Υ+(k)

+H∗(k)[

D−1(k)+G∗(k)Υ+(k)G(k)]

H(k)

= Υ+(k)−Z(k)H(k)+H∗(k)[{

D−1(k)+G∗(k)Z(k)}

︸︷︷︸L(k)

H(k)−Z∗(k)

︸︷︷︸U(k)

]

In other words, defining the intermediate terms Z(k), L(k) and U(k) based on theabove grouping, we have:

Z(k)�= Υ+(k)G(k)

L(k)�= D−1(k)+G∗(k)Z(k) = D−1(k)+G∗(k)Υ+(k)G(k)

U(k)�= L(k)H(k)−Z∗(k)

Υ(k) = Υ+(k)+H∗(k)U(k)−Z(k)H(k)

(10.21)

The restructured computations in (10.21) are computationally less expensive com-pared with computing the τ∗(k)Υ+(k)τ(k) product directly. (10.19) and (10.21),lead to the O(N) base-to-tips scatter recursive Algorithm 10.3 for computing theΥ(i) block-diagonal elements. This algorithm requires the D(k) articulated body

Algorithm 10.3 Computation of the Υ(k) elements

First compute the D(k) articulated body inertia elements using the tips-to-basegather recursion in Lemma 9.7. Then carry out the following base-to-tips scatterrecursion for the Υ(k) elements:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩


Υ+(k) = φ∗(℘(k),k)Υ(℘(k))φ(℘(k),k)

Z(k) = Υ+(k)G(k)

L(k) = D−1(k)+G∗(k)Z(k)

U(k) = L(k)H(k)−Z∗(k)Υ(k) = Υ+(k)+H∗(k)U(k)−Z(k)H(k)

end loop

(10.22)

hinge inertia for all the links. Two recursions are thus needed to compute Υ sinceP, D, and Eψ must be first computed using the tips-to-base gather recursion fromLemma 9.7.


Remark 10.1 Singularity of Υ(k) matrices.A point worth highlighting is that the Υ(k) operational space compliance kernelmatrices, while symmetric and positive semi-definite, may be singular. To illustratethis, let us assume that the system has single degree of freedom hinges. In this case,the H(k) matrices have rank 1 and, consequently, so do the H∗(k)D−1(k)H(k)terms in (10.22). At the base, theΥ(n) =H∗(n)D−1(n)H(n), and this 6×6 matrixhas a rank of just 1 and is, thus, singular. As the recursion proceeds towards the tips,theH∗(k)D−1(k)H(k) contribution of each link can increase the rank ofΥ(k) by atmost 1. At least 6 links need to be traversed before Υ(k) can be expected to becomenon-singular.

Physically, this means that the nth body has only 1 degree of freedom and canmove in only one direction. The body has zero compliance (or infinite stiffness) formotions along the constrained directions. In general, the null space of the Υ(k)matrix defines the directions along which the kth body cannot move. Each newoutboard body acquires additional mobility, as reflected in the increasing rank ofits Υ(k) matrix. It takes an accumulation of at least 6 degrees of freedom from theancestor bodies to allow a body to be movable in arbitrary directions and for itsΥ(.) matrix to become non-singular. Even after 6 links, at singular configurations,mobility in some directions can be lost and the Υ(k) matrix then becomes singular.

Remark 10.2 P−1(n) = Υ(n) for a free-flying system.For a free-flying system, the base-body hinge is a full 6 degree of freedom hinge withH(n) = I. It follows from (10.16) that

Υ(n) =H∗(n)D−1(n)H(n) = D−1(n)

= [H(n)P(n)H∗(n)]−1 = P−1(n)

At the base-body, Υ(n) is simply the inverse of the P−1(n) articulated body inertiaof the link!

Section 17.4 on page 348 contains a more extended discussion of the specialproperties of the Υ(k) matrices for free-flying systems. It shows that the Ω and Υmatrices are invariant with respect to the choice of base-body. It also derives thealternative Algorithm 17.5 for computing the Υ elements by combining values frommultiple articulated body inertia sweeps. One implication is that all of the Υ(k)matrices are always invertible for free-flying systems.

The following exercise looks at a scenario where one tree-topology system isattached to a body on another system. Examples of such systems are micro/macromanipulator systems [27], where a lightweight (micro) manipulator is mounted onthe end-effector of a macro manipulator to create a system capable of large scalemotions as well as agile, fine-manipulation tasks. This exercise explores the opera-tional space properties of such coupled systems.


Exercise 10.1 Υ(k) for a micro/macro manipulator system.We now study a system consisting of a S “micro” manipulator mounted on theend-effector body of a P “macro” manipulator. Denote the end-effector body of themacro system as the ℘(S) body. Show that the Υ(1) end-effector operational spacecompliance kernel matrix for the combined system is related toΥ(℘(S)) as follows:

Υ(1) = ΥS(1)+ψ∗(P(℘(S),1)Υ(℘(S))ψ(P(℘(S),1) (10.23)

In the above, Υ(1) is associated with the combined system, ΥS(1) denotes the cor-responding matrix for the micro manipulator by itself. Υ(℘(S)), on the other hand,is the operational space compliance kernel for the combined system at the ℘(S)body, i.e., at the end-effector of the macro system. In particular, (10.23) implies that

Υ(1) � ΥS(1) (10.24)

Physically, this implies that the end-effector of the combined system is alwaysmore compliant than the end-effector of the micro system alone regardless of thestiffness of the macro system. From the OSC control perspective, this suggests astrategy of mounting a fine motion micro-manipulator at the end-effector of a large-motion macro-manipulator. The overall system will have the large workspace ofthe macro-manipulator while retaining the fine-motion control and dexterity of themicro-manipulator.

10.2.5 Simplifications for Serial-Chain Systems

Now, we examine simplifications of Ω that arise for serial-chain systems. The keycharacteristic that distinguishes serial-chain systems from tree-topology systems isthat all bodies are related. Thus, for any pair of distinct bodies in the system, one ofthe bodies is always an ancestor of the other. This leads to the following version ofLemma 10.1, using the simplifications in Lemma 9.6 on page 172 for serial-chainsystems. One of the key simplifications is that E∗

ψΥEψ contains no off-diagonalterms for serial-chain systems. Without loss in any generality, the serial-chain isassumed to be canonical.

Lemma 10.2 Decomposition of Ω for serial-chains.For a serial-chain system, Ω can be decomposed into disjoint diagonal, strictlyupper triangular and strictly lower triangular parts, as follows:

Ω= Υ+ ψ∗Υ+Υψ (10.25)

where Υ ∈R6n×6n is a block-diagonal operator satisfying the following backwardLyapunov equation:

H∗D−1H = Υ−E∗ψΥEψ (10.26)


The 6× 6 dimensional, symmetric positive semi-definite Υ(k) diagonal matricessatisfy the parent/child recursive relationship in (10.15), and can be computed viathe following O(N) base-to-tips scatter recursion:

⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

Υ(n+ 1) = 0

for k = 1 · · ·nΥ(k) =ψ∗(k+ 1,k)Υ(k+ 1)ψ(℘(k),k)+H∗(k)D−1(k)H(k)

end loop

(10.27)

While Υ defines the block-diagonal elements of Ω, the following recursive expres-sions describe all the terms, including the off-diagonal ones:

Ω(i, j) =

⎧⎪⎨⎪⎩Υ(i) for i= j

Ω(i, j+ 1)ψ(j+ 1, j) for i > j

Ω∗(j, i) for i < j

(10.28)

Proof: This lemma follows from applying the serial-chain simplifications fromLemma 9.6 on page 172 to Lemma 10.1.

10.2.6 Explicit Computation of the Mass Matrix Inverse M−1

We have the following options for explicitly computing the mass matrix inverse ofa serial-chain system:

1. Explicitly compute the M mass matrix and invert this matrix numerically (O(N3)cost).

2. Compute the mass matrix factors of M−1 in (9.51) and evaluate their products(O(N3) cost).

The following exercise uses the decomposition of Ω in Lemma 10.2 to develop asimpler O(N2) algorithm for computing the mass matrix inverse for a canonicalserial-chain system.

Exercise 10.2 Computation of the mass matrix inverse.

1. Derive the following operator decomposition of the mass matrix inverse for atree-topology system:

M−1 = L−K∗ψ∗U∗ −UψK+K∗RK (10.29)

where L and U are defined as

Υ�= E∗

φΥEφ, L�= D−1 +G∗ΥG and U

�= LH−G∗Υ (10.30)

10.3 The Operational Space Cos Coriolis/Centrifugal Term 199

2. Show that, for a serial-chain system, (10.29) simplifies to the following disjointdecomposition into block-diagonal, strictly upper-triangular and strictly lower-triangular terms:

M−1 = L−K∗ψ∗U∗ −UψK and Υ= Υ+ (10.31)

Since Υ = E∗φΥEφ is block-diagonal for serial-chain systems, so are L and U.

Indeed, the block-diagonal elements of L and U are the L(k) and U(k) elementsdefined in (10.21). A closer examination shows that the elements of M−1 aregiven by:

M−1(k, j) =

⎧⎪⎨⎪⎩

−U(k)ψ(k, j+ 1)K(j+ 1, j) for k > j

L(k) for k= j

M−1(j,k) for k < j

(10.32)

Use this to derive the O(N2) Algorithm 10.4 for explicitly computing the massmatrix inverse of a serial-chain system.

The use of the operational space compliance kernel elements for the computationof the mass matrix inverse is an example of its use for a non-OSC application.

10.3 The Operational Space Cos Coriolis/Centrifugal Term

In this section we derive simpler expressions for the Cos Coriolis terms in (10.8)[110]. First, we begin by introducing the U and U⊥ projection operators.

10.3.1 The U and U⊥ Projection Operators

Define the pair of operators, U and U⊥, as follows:

U�= ΩM and U⊥

�= I−U = I−ΩM (10.34)

The following lemma shows that U and U⊥ are in fact projection operators.

Lemma 10.3 The U and U⊥ projection operators.The operators U and U⊥ are projection operators, i.e.,

U2 = U, and U2⊥ = U⊥ (10.35)


Algorithm 10.4 O(N2) Computation of the mass matrix inverse for a serial-chainThis algorithm assumes that the serial-chain is canonical for simpler notation. Thus,℘(k) = k+ 1. It also assumes that the tips-to-base gather algorithm for the articu-lated body inertia quantities has been completed. The following recursive procedurecomputes all the operational space compliance kernel terms and the elements of themass matrix inverse:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

Υ(n+ 1) = 0

for k = n · · ·1Υ+(k) = φ∗(k+ 1,k)Υ(k+ 1)φ(k+ 1,k)

Z(k) = Υ+(k)G(k)

M−1(k,k) = L(k) = D−1(k)+G∗(k)Z(k)

U(k) = L(k)H(k)−Z∗(k)Υ(k) = Υ+(k)+H∗(k)U(k)−Z(k)H(k)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

X(k− 1) =U(k)φ(k,k− 1)

for j = k− 1 · · ·1M−1(k, j) =

[

M−1(j,k)]∗

= −X(j)G(j)

X(j− 1) = X(j)τ(j)φ(j, j− 1)

=[

X(j)+M−1(k, j)H(j)]

φ(j, j− 1)

end loop

end loop

(10.33)

Also,

UΩ=Ω and U⊥Ω = 0

U∗⊥MU = 0, U∗MU = MU, U∗

⊥MU⊥ = MU⊥(10.36)

Proof: We have

ΩMΩ10.12= ψ∗H∗D−1[HψMψ∗H∗]D−1Hψ

9.46= Ω (10.37)

Therefore,U2 10.34

= ΩMΩM10.37= ΩM

10.34= U

This implies that U is a projection operator. Since U is a projection operator, so isU⊥ = I−U.UΩ=Ω follows from (10.37), and this, in turn, directly implies that U⊥Ω= 0.Moreover,

U∗MU = MΩMΩM10.37= MΩM = MU

The remaining identities in (10.36) follow directly from the above result.


Lemma 10.4 Useful identities for U and U⊥.

1.

Uφ∗ = ΩMφ∗ = φ∗ −(ψ∗ −ΩP)

= φ∗H∗ [K∗(ψ∗ −ΩP)+D−1HψP]

(10.38a)

Uψ∗ = ΩMψ∗ =ΩP (10.38b)

(ψ−PΩ)Mφ∗ = (ψ−PΩ)P+Pψ∗ =ψMψ∗ −PΩP (10.38c)

2.

U⊥φ∗ = U⊥ψ∗ =ψ∗ −ΩP (10.39a)

U⊥φ∗H∗ = U⊥ψ∗H∗ = 0 (10.39b)

Proof:

1. We have

Uφ∗ 10.34= ΩMφ∗ 10.12

= ψ∗H∗D−1HψMφ∗ 9.41= ψ∗H∗D−1H[ψP+Pφ∗]

9.32,9.37,10.12= ΩP+ψ∗H∗K∗φ∗ 9.43

= φ∗ −(ψ∗−ΩP)

This establishes the first equality in (10.38a).Also,

φ∗H∗K∗(ψ∗ −ΩP)10.12= φ∗H∗K∗ψ∗[I−H∗D−1HψP]

9.43= (φ∗ −ψ∗)[I−H∗D−1HψP]

10.12= φ∗ −(ψ∗ −ΩP)−φ∗H∗D−1HψP

10.38a= Uφ∗ −φ∗H∗D−1HψP

This establishes the second equality in (10.38a).Now,

Uψ∗ 10.34= ΩMψ∗ 10.12

= ψ∗H∗D−1HψMψ∗ 9.42= ψ∗H∗D−1H

(

ψP+Pψ∗)

9.32,8.19= ΩP+ψ∗τ∗E∗

ψψ∗ 9.34

= ΩP+ψ∗τ∗τ∗E∗φψ

∗ 9.32= ΩP

establishing (10.38b).


Additionally

(ψ−PΩ)Mφ∗ 9.41,10.38a= ψP+Pφ∗ −P[φ∗ −ψ∗+ΩP]

= ψP+Pψ∗−PΩP9.42= ψMψ∗ −PΩP

This establishes (10.38c).

2. The equalities in (10.39a) follow directly from (10.38a) and (10.38b).For (10.39b),

(ψ− I)PH∗ 8.19= ψEψPH∗ 9.34

= ψEφτPH∗

9.32= ψEφ(I−GH)PH∗ 9.32

= 0 ⇒ ψPH∗ = PH∗(10.40)

Hence,

ΩPH∗ 10.12= ψ∗H∗D−1HψPH∗ 10.40

= ψ∗H∗D−1HPH∗ 9.32= ψ∗H∗

Post-multiplying (10.39a) byH∗, and using the above, establishes (10.39b).

10.3.2 Computing Cos

Before proceeding to the problem of computing Cos observe that the general formof the C generalized force vector in (10.8), to include external forces and gravity, isgiven by the following expression from (5.37) and (5.39):

C(θ, θ) =Hφ [Mφ∗(a+E∗g)+b] (10.41)

Lemma 10.5 Operator expression for Cos.Assuming Λ exists, the Cos operational space inertia Coriolis term is given by thefollowing expression:

Cos =Λ(

B∗[ψ∗{H∗D−1Hψ(Pagr+b+KT)

− (agr+H∗D−1T)}

+ E∗g]

− B∗V) (10.42)

with

agr�= a+E∗g (10.43)


Proof:From (10.8),

Cos10.8= Λ

(

JM−1(C−T)−aos)

3.53,10.41,9.51= Λ

(

B∗φ∗H∗[I−HψK]∗D−1[I−HψK]

∗(

Hφ[

Mφ∗agr+b]

−T)

−aos

)

9.45,10.12,10.3= Λ

(

B∗Ω[

Mφ∗agr+b+KT]

−aos−B∗ψ∗H∗D−1T)

10.38a,10.3= Λ

(

B∗[(φ∗ −ψ∗+ΩP)agr+Ω(b+KT)]

−[

B∗φ∗a+ B∗V]

−B∗ψ∗H∗D−1T)

10.43= Λ

(

B∗[(−ψ∗ +ΩP)a+(φ∗−ψ∗ +ΩP)E∗g+Ω(b+KT]

− B∗V−B∗ψ∗H∗D−1T

)

10.43= Λ

(

B∗[Ω(Pagr+b+KT)−ψ∗(agr+H∗D−1T)+φ∗E∗g]

− B∗V)

5.41,10.12= Λ

(

B∗[ψ∗{H∗D−1Hψ(Pagr+b+KT)

− (agr+H∗D−1T)}

+ E∗g]

− B∗V)

The expression in (10.42) can be broken down into the following intermediateset of expressions:

ϑ�= ψ(Pagr+b+KT)

η�= ψ∗ (H∗D−1(Hϑ−T)−agr

)

ζ�= η+ E∗g

Cos =Λ(

B∗ζ− B∗V)

(10.44)

The recursive algorithm implied by (10.44) is shown in Algorithm 10.5.This O(N) algorithm consists of tips-to-base gather sweep followed by a base-

to-tips scatter sweep, followed by the solution of a linear matrix equation. This algo-rithm requires the articulated body inertia quantities, as well asΛ. If these quantitiesare not available, then the tips-to-base gather sweep can be modified to also com-pute the articulated body inertia quantities, while the subsequent base-to-tips scattersweep can be modified to compute the components of Λ.


Algorithm 10.5 The operational space Coriolis/centrifugal force term

⎧⎪⎪⎪⎨⎪⎪⎪⎩


ϑ(k) =∑

∀j∈�(k)

(

ψ(k, j)ϑ(j)++K(k, j)T(j))

+P(k)agr(k)+b(k)

end loop⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩


η(k) =ψ∗(℘(k),k)η(℘(k))+H∗(k)D−1(k)[

H(k)ϑ(k)−T]

−agr(k)

ζ(k) = η(k)+g

end loop

Cos = Λ(

B∗ζ− B∗V)

10.4 Divide and Conquer Forward Dynamics

The Divide and Conquer Algorithm (DCA) [49] is a parallelizable procedure forsolving the forward dynamics problem. The algorithm is based on the idea that theequations of motion of a system can be assembled from the equations of motion ofcomponent sub-systems. In this context, hinges are viewed as constraints betweenindependent component systems. Lemma 10.6, below, describes the procedure forcombining the operational space dynamics of a pair of independent sub-systems intoone where the systems are coupled by a hinge. This result forms the basis for theDCA forward dynamics algorithm, which is outlined subsequently.

To begin, let us assume that we have a pair of independent systems A and Bwhose equations of motion are given by1:

ΛAfAnd+ CAos = βA

and ΛBfBnd+ CBos = βB

(10.45)

Initially, A and B systems are assumed to be uncoupled, and the fAnd and fBnd arestacked vectors of external forces at a pair of handles, 1 and 2, on each of the sys-

tems, as illustrated in Fig. 10.2. βA

and βB

denote the stacked spatial accelerationvectors of the handles, and ΛA and ΛB denote the operational space compliancematrix for the pair of systems. The handles are used to partition these equations ofmotion as follows:

1 The form of these equations are similar, but not identical, to the operational space inertia form in(10.7) on page 188. We adopt this alternate form since it is closer to that in Featherstone [49].

10.4 Divide and Conquer Forward Dynamics 205

A2handle

A1handle

B2handle

B1handle

Constraint Hinge

Multibody System BMultibody System A

Fig. 10.2 Illustration of aA andB component systems and the their inter-coupling througha hinge between theA2 and B2 handles on them

(

ΛA11 ΛA12

ΛA21 ΛA22

)[

fAnd1

fAnd2

]

+

[

CAos1

CAos2

]

=

[

βA

1

βA

2

]

and

(

ΛB11 ΛB12

ΛB21 ΛB22

)[

fBnd1

fBnd2

]

+

[

CBos1

CBos2

]

=

[

βB

1

βB

2

]

(10.46)

The following lemma describes the overall equations of motion resulting from con-necting the two systems via a hinge constraint between one handle each on theA andB systems.

Lemma 10.6 Combined operational space equations of motion.Let A and B be systems with operational space equations of motion defined by(10.46). Let the systems be connected via a constraint in the form of a hinge betweenthe A2 and B2 pair of handles – one on each of the A and B systems. Assume thatR is the joint map matrix for the hinge, with orthogonal complement matrix P (i.e.,PR= 0). Thus,

(βA

2 − βB

2 ) = Rθ and P(βA

2 − βB

2 ) = 0 (10.47)

θ denotes the generalized acceleration of the constraint hinge. The equations ofmotion of the combined system are given by the following expressions:

ΛC

[

fAnd1

fBnd1

]

+ CCos =

[

βA

1

βB

1

]

where W�= P∗

[

P(ΛA22 +ΛB22)P∗]−1

P

and ΛC =

(

ΛA11 00 ΛB11

)

−

[

ΛA12

−ΛB12

]

W

[

ΛA21, −ΛB21

]


and CCos =

[

CAos1

CBos1

]

−

[

ΛA12

−ΛB12

]

(

W[

CAos2 − CBos2

]

−[

I−W(

ΛA22 +ΛB22

)]

RT

)

(10.48)

with T denoting the generalized force at the hinge.Proof: The constraint in (10.47) can be expressed as:

P[I, −I]

[

βA

2

βB

2

]

= 0 (10.49)

The dual version of this equation applies to the forces between the pair of systemsand takes the form:

[

fAnd2

fBnd2

]

=

[

I

−I

]

(P∗λ+RT) (10.50)

for some λ Lagrange multiplier vector. Combining the component equations in(10.46) leads to:

⎛

⎜

⎜

⎜

⎝

ΛA11 ΛA12 0 0

ΛA21 ΛA22 0 00 0 ΛB11 ΛB12

0 0 ΛB21 ΛB22

⎞

⎟

⎟

⎟

⎠

⎡

⎢

⎢

⎢

⎣

fAnd1

fAnd2

fBnd1

fBnd2

⎤

⎥

⎥

⎥

⎦

+

⎡

⎢

⎢

⎢

⎣

CAos1

CAos2

CBos1

CBos2

⎤

⎥

⎥

⎥

⎦

=

⎡

⎢

⎢

⎢

⎢

⎣

βA

1

βA

2

βB

1

βB

2

⎤

⎥

⎥

⎥

⎥

⎦

(10.51)

Rearranging the rows and columns of (10.51) so as to gather the constrained rowsand columns yields:

⎛

⎜

⎜

⎜

⎝

ΛA11 0 ΛA12 0

0 ΛB11 0 ΛB12

ΛA21 0 ΛA22 0

0 ΛB21 0 ΛB22

⎞

⎟

⎟

⎟

⎠

⎡

⎢

⎢

⎢

⎣

fAnd1

fBnd1

fAnd2

fBnd2

⎤

⎥

⎥

⎥

⎦

+

⎡

⎢

⎢

⎢

⎣

CAos1

CBos1

CAos2

CBos2

⎤

⎥

⎥

⎥

⎦

=

⎡

⎢

⎢

⎢

⎢

⎣

βA

1

βB

1

βA

2

βB

2

⎤

⎥

⎥

⎥

⎥

⎦

(10.52)

Taken together, (10.49), (10.50), and (10.52) are in a form matching (A.16) onpage 400 with the following identifications:

A=

(

ΛA11 0

0 ΛB11

)

, B=

(

ΛA12 0

0 ΛB12

)

, c1 =

[

CAos1

CBos1

]

, c2 =

[

CAos2

CBos2

]

C=

(

ΛA21 00 ΛB21

)

, D=

(

ΛA22 00 ΛB22

)

, b1 =

[

βA

1

βB

1

]

, b2 =

[

βA

2

βB

2

]

Q= P[I, −I], γ=

[

I

−I

]

RT

10.4 Divide and Conquer Forward Dynamics 207

Thus,

QDQ∗ = P[

ΛA22 +ΛB22

]

P∗, and Z=

[

ΛA12

−ΛB12

]

W [I, −I]

With the above identifications, we can directly apply the solution for (A.16) to thisproblem to obtain the following solution for (10.52):

ΛC

[

fAnd1

fBnd1

]

+ CCos =

[

βA

1

βB

1

]

where the expressions forΛC and CCos are as follows:

ΛC�= A−ZC=

(

ΛA11 00 ΛB11

)

−

[

ΛA12

−ΛB12

]

W

[

ΛA21, −ΛB21

]

CCos�= c1 +Bγ−Z(c2 +Dγ)c1

= c1 +B

[

I

−I

]

(

RT −W[

CAos2 − CBos2

]

−W(

ΛA22 +ΛB22

)

RT)

=

[

CAos1

CBos1

]

−

[

ΛA12

−ΛB12

]

(

W[

CAos2 − CBos2

]

−[

I−W(

ΛA22 +ΛB22

)]

RT

)

This establishes the result.ΛC represents the operational space compliance matrix of the combined system,

and (10.48) expresses it in terms of the operational space compliance matrices ofthe individual systems and the hinge constraint between them.

Exercise 10.3 Ground connected equations of motion.Use Lemma 10.6 to show that, if the A2 handle for system A is connected to the in-ertial frame through the hinge (instead of to system B), then the resulting equationsof motion are given by the following expressions:

ΛCfAnd1 + CCos = βA

1 where W�= P∗

[

PΛA22P∗]−1

P

and ΛC = ΛA11 −ΛA12WΛA21

with CCos = CAos1 −ΛA12

(

WCAos2 −[

I−WΛA22

]

RT)

(10.53)

We have seen how the operational space dynamics of a pair of independent sys-tems can be used to obtain the operational space dynamics when the systems are


coupled by a hinge. The expression for ΛC was first derived in Featherstone [49]as a step towards the development of the parallelizable DCA forward dynamicsalgorithm. The DCA begins by treating each individual body in the system as anindependent system with handles defined by the hinge connection nodes. The Λmatrix for each of these single body systems is simply the inverse of their individ-ual spatial inertias. Neighboring bodies can then be combined using Lemma 10.6 todevelop the operational space dynamics of the pairs of bodies. This assembly pro-cedure is continued, combining pairs of these systems, until the operational spacecompliance matrix of the complete system is assembled. If the system is connectedto the ground, then (10.53) can be used to attach this final hinge. The resulting DCAis outlined in Algorithm 10.6.

Algorithm 10.6 The DCA forward dynamics algorithmThe following describes the Divide and Conquer algorithm for tree systems:

1. Carry out a binary tree assembly sweep that begins with the formation of theoperational space compliance matrix of individual bodies and uses Lemma 10.6to couple them in each subsequent pass to build larger and larger assemblies.The assembly sweep terminates when the operational space compliance matrixfor the whole system has been obtained.

2. This sweep is a back-substitution pass that proceeds in the reverse direction bysolving for, and propagating, the interaction spatial forces and generalized accel-erations across the connecting hinges of the component sub-assemblies.

Since the assembly and the back-substitution passes across the component sub-assemblies can be carried out independently, the DCA procedure can be parallelized.While a serial implementation of this algorithm is more expensive than the AB for-ward dynamics algorithm, its computational cost is just O(log(n)) for a parallelimplementation on O(n) processors. Numerical accuracy and other issues of theDCA technique are discussed in [49, 50].

Chapter 11Closed-Chain Dynamics

The subject of this chapter is the dynamics of closed-chain multibody systems.These are systems whose underlying digraphs are not trees, but may be multiply-connected and include directed cycles, as illustrated in Fig. 8.6 on page 152. Section9.6.2 on page 185 discussed some of the hurdles in extending SKO models to closed-chain systems. The issues identified included the fact that the SKO operator is notnilpotent when the digraph has directed cycles, and that the hinge generalized coor-dinates are not independent when there are multiply-connected nodes in the systemdigraph.

A common approach for handling such digraph systems is to decompose theminto a spanning tree and a set of cut-edges representing motion constraints on thefree dynamics of the spanning tree.

11.1 Modeling Closed-Chain Dynamics

Since the spanning tree for the digraph of the closed-chain system is a tree, an SKOmodel for it can be defined using the techniques from Chap. 9, with equations ofmotion expressed as

M(θ)θ+C(θ, θ) = T (11.1)

This spanning tree SKO model has N dimensional configuration, and N dimensionalvelocity, degrees of freedom. The effect of the cut-edge motion constraints is toreduce the degrees of freedom in the system to less than N.

We will focus on the handling of bilateral constraints, and discuss the extensionsneeded for unilateral constraints in Sect. 11.5.

11.1.1 Types of Bilateral Motion Constraints

We will consider bilateral holonomic and non-holonomic motion constraints.


210 11 Closed-Chain Dynamics

Holonomic constraints: When the motion constraints can be expressed purelyas a function of the generalized coordinates, the constraints are referred to asholonomic constraints. These constraints are defined by constraint equations ofthe following form:

d(θ,t) = 0 (11.2)

where d(.,t) is an nc-dimensional constraint function. Observe that the holo-nomic constraint equation does not depend upon the generalized velocities, butcan depend on the generalized coordinates, and can be time varying. If the holo-nomic constraint function d(., .) is time-invariant, then the holonomic constraintsare said to be scleronomic. When this is not the case, the constraints are said tobe rheonomic.Differentiating (11.2) with respect to time leads to the following constraint equa-tion in the velocity domain:

d(θ,t) = Gc(θ,t)θ−U(t) = 0 (11.3)

where

Gc(θ,t)�= ∇θd(θ,t) ∈Rnc×N and U(t)

�= −

∂d(θ,t)∂t

∈Rnc (11.4)

This differential form of constraints are also referred to as a Pfaffian form. WhenU = 0, the differential form of the constraints is said to be catastatic. When thisis not the case, the constraints are said to be acatastatic. Clearly, scleronomicholonomic constraints lead to catastatic differential constraints, while rheonomicones lead to acatastatic ones.We assume that Gc(θ,t) is a full-rank matrix. Some notable points about (11.3)are that it is integrable, nc-dimensional, and equivalent (modulo a constant) to(11.2). Also, it is linear in the generalized velocity coordinates. These constraintseffectively reduce the generalized coordinates and velocities for the system fromN to (N−nc) dimensional manifolds and vector spaces, respectively.

Non-holonomic constraints: When the definition of the constraints involvesnon-integrable constraints on the generalized velocities, then the constraints aresaid to be non-holonomic. For such constraints, the motion constraints are ex-pressed directly in the (11.3) Pfaffian form without the additional configuration-level constraints in (11.2) and (11.4). In the case of non-holonomic constraints,the dimension of the configuration space remains unchanged at N [153], andonly the dimension of the generalized velocity subspace is reduced to N−nc.We will be restricting our attention here to only non-holonomic constraints aris-ing from equality constraints on the permissible velocities, even though moregenerally, inequality constraints (e.g., unilateral constraints) on the configurationof a system are also classified as non-holonomic constraints on the system.

Thus, holonomic constraints lead to a reduction in the number of configuration de-grees of freedom and the motion degrees of freedom by equal amounts. On the other

11.1 Modeling Closed-Chain Dynamics 211

hand, non-holonomic constraints only lead to a reduction in the number of motiondegrees of freedom, and leave the number of configuration degrees of freedom un-changed. In other words, holonomic constraints reflect the global behavior of thehinge, while non-holonomic constraints reflect only the local behavior. A detaileddiscussion on holonomic and non-holonomic constraints can be found in Rosenberg[153].

We deal with both holonomic and non-holonomic motion constraints in this chap-ter, using the velocity domain characterization of (11.3) as the common point ofdeparture.

11.1.2 Constrained System Forward Dynamics Strategies

The effect of the cut-edge constraints can be incorporated into the SKO model forthe spanning tree via Lagrange multipliers, denoted λ ∈Rnc . Augmenting (11.1)with the Lagrange multipliers results in the following overall equations of motionfor the closed-chain system:

M(θ)θ+C(θ, θ)−G∗c(θ,t)λ = T

Gc(θ,t)θ = U(t)(11.5)

The −G∗c(θ,t)λ term in the first equation above represents the internal generalized

constraint forces from the motion constraints.By differentiating the constraint equation, (11.5) can be rearranged into the fol-

lowing matrix form:(

M G∗c

Gc 0

)[

θ

−λ

]

=

[

T −C

U

]

where U�= U(t)− Gc θ ∈Rnc (11.6)

Observe thatd(θ,t)

11.3,11.6= Gc θ−U (11.7)

The following options are available for solving the forward dynamics of systemswith closure constraints:

Augmented approach: The augmented approach treats the cut-edge motion con-straints as a correction to be applied to the dynamics of the spanning tree sys-tem. The SKO techniques and algorithms can be applied to efficiently solvethe dynamics of the SKO model for the spanning tree. Additional steps are re-quired to account for the motion constraints described by the cut-edges. Thistechnique works with the larger N-dimensional set of generalized velocity co-ordinates rather than with the N −nc truly independent ones for the system.The use of non-minimal coordinates requires the use of differential-algebraicequation (DAE) integrators, along with error-control techniques for mitigatingthe solution’s drift away from the constraint manifold. More precisely, (11.6) is


referred to as an index-3 DAE formulation of the equations of motion [13]. Theaugmented dynamics approach is described in Sect. 11.2.

Direct approach: The direct approach solves (11.6) for the unknown (θ,λ) us-ing matrix solvers. Since we have assumed thatGc is full-rank, the matrix on theleft is non-singular and the equation has a unique solution. This solution tech-nique is typically used with absolute instead of relative coordinate formulations.In the absolute coordinates formulation, the mass matrix block becomes a simpleblock-diagonal constant matrix, while λ grows to include the closure, as well asthe inter-link hinge constraints [132]. While the system dimensionality is muchlarger, and the solution complexity is ostensibly of O(N3) complexity, solversexploiting the sparsity of the coefficient matrix can reduce the computationalcost. Like the augmented approach, this technique uses non-minimal generalizedcoordinates, and is subject to similar DAE integration and error-control issues.Schwerin [158] contains an in-depth discussion of this closed-chain system dy-namics approach.

Projected approach: The projected approach uses numerical techniques to elim-inate the constraints from the equations of motion [121, 158]. This approachworks with minimal coordinates that automatically satisfy the cut-edge con-straints. There is no attempt to preserve the topological structure of the systemduring the constraint elimination process and, consequently, the resulting equa-tions of motion do not satisfy the requirements for an SKO model. Thus, theSKO analytical and low-order algorithmic techniques are not available to thisapproach. This procedure is described in Sect. 11.3. Section 11.4 discusses theequivalence of the augmented and projected dynamics formulations.

Constraint embedding approach: The constraint embedding approach trans-forms the original digraph into a tree graph by aggregating the “non-tree” sub-graphs in the system into nodes. Similar to the projected technique, this approachalso works with minimal coordinates. One notable feature is that multiple physi-cal bodies are assigned to individual nodes in the transformed tree. This is a de-parture from the standard digraphs, where there is a one-to-one correspondencebetween the physical bodies and the nodes in the digraph. All the SKO tech-niques, including low-order forward dynamics, are applicable to the SKO modelfor the transformed tree. Another benefit of this approach is that the minimal ve-locity coordinate model is more suitable for control applications. The constraintembedding technique is described in Chap. 15.

11.2 Augmented Approach for Closed-Chain Forward Dynamics

Lemma 11.1 Augmented closed-chain forward dynamics solution.The closed-chain dynamics generalized accelerations in (11.6) can be expressed as

θ = θf+ θδ (11.8)

11.2 Augmented Approach for Closed-Chain Forward Dynamics 213

where, the free generalized accelerations, θf, the correction generalized accelera-tions, θδ, and the Lagrange multipliers, λ, are given by

θf�= M−1 (T −C) (11.9a)

λ = −[

GcM−1G∗

c

]−1df where df

�= Gc θf−U (11.9b)

θδ�= M−1G∗

c λ (11.9c)

Proof: Since M is invertible, (11.6) satisfies the requirements of the (A.9) matrixequation on page 398. The result follows by using its solution in (A.10) to obtain(11.9).

The θf = M−1(T −C) term represents the generalized accelerations solution forthe dynamics of the spanning tree SKO model while ignoring the closure constraintsand, is therefore referred to as the free generalized accelerations. df represents theacceleration-level constraint violation resulting from just the free dynamics of thesystem. The solution to the closed-chain forward dynamics conceptually involvesthe steps described in Algorithm 11.1. It is noteworthy that the solution for the θf

Algorithm 11.1 Augmented forward dynamics algorithm

1. Solve (11.9a) for the θf free generalized accelerations for the SKO model for thespanning tree.

2. Use θf in (11.9b) to solve for the λ Lagrange multipliers.3. Use λ to solve (11.9c) for the θδ correction accelerations,4. Compute the generalized accelerations using (11.8).

free accelerations requires the solution of the forward dynamics of the SKO modelfor the spanning tree, for which the efficient O(N) AB procedure in Algorithm9.5 on page 182 is available. The augmented formulation solves for the full N

dimensional θ generalized accelerations vector even though only N −nc of theelements are in fact truly independent. One numerical consequence of this over-parametrization is that DAE integrators, instead of the ODE integrators for tree sys-tems, are required for the numerical integration of the accelerations and velocities[14, 158, 173]. Furthermore, constraint error control techniques are needed to man-age the inevitable numerical errors that cause the system state to drift off of theconstraint manifold [24, 33, 158].

A direct relationship between the T applied torques and the θ generalized accel-erations is the subject of the following exercise.

Exercise 11.1 Mapping between T and θ for closed-chain systems.Show that the relationship between T and θ for closed-chain systems is given by

θ = YC[T −C]+M−1G∗c

[

GcM−1G∗

c

]−1U (11.10)


where

YC�= M−1 −M−1G∗

c

[

GcM−1G∗

c

]−1GcM

−1 ∈RN×N (11.11)

Observe that[

GcM−1G∗

c

]−1GcM

−1 is a generalized inverse of G∗c. Since YC

defines a symmetric and positive semi-definite mapping between T and θ, it istempting to think of it as being the mass matrix inverse for the constrained sys-tem. However, YC is singular and not invertible. This can be seen by noting that

YCG∗c = 0 (11.12)

Since YC is singular, there is no well-defined mass matrix for the constrained sys-tem. The singularity of YC results from the fewer than N velocity degrees of free-dom in the system.

11.2.1 Move/Squeeze Decompositions

For tree-topology systems, any non-zero generalized force results in non-zero accel-erations in the system. This is not the case for constrained systems; these can havegeneralized forces that induce no accelerations in the system at all! The motion de-grees of freedom that are lost in the presence of constraints are recovered in theforce domain. In other words, the generalized forces can be used to manage an nc-dimensional sub-space of non-motion inducing internal forces within the system.This is useful during robotics manipulation and control tasks, where it is requiredto manage the motion, as well as the forces, in the system. Examples include taskswith manipulator end-effectors in contact with the environment, multi-arm systemsmanipulating objects, and locomotion involving legged systems.

From (11.10) and (11.12), it follows that any T in the column space of G∗c will

result in θ = 0, i.e., it will not induce any motion in the system. We refer to suchgeneralized forces as squeeze generalized forces since their presence effects in-ternal forces within the system but induces no motion. We use the Tsq notation todenote squeeze generalized forces.

The complement of the subspace of squeeze forces is referred to as the subspaceof move generalized forces. move forces always result in non-zero motion in thesystem. We use the Tmv notation for move forces. We can decompose any general-ized force vector, T, into move and squeeze components as follows:

T = Tmv+Tsq (11.13)

This decomposition is not unique. The component forces on the right can be ob-tained by defining a move/squeeze projection matrix, Pms, as

Pms�= G∗

c(XG∗c)

−1X ∈RN×N (11.14)


where X is an nc×N matrix such that (XG∗c) is invertible. Possible choices for X

include GcW, with W being a symmetric positive-definite matrix such as W = I,or an appropriate constant weight matrix.

Using the Pms projection, any generalized forces vector T can be expressed inthe form

T = Tmv+Tsq where Tsq�= PmsT, Tmv

�= (I−Pms)T (11.15)

The move/squeeze decomposition has useful implications for coordinated motionand force control [108, 109, 181]. The control system can be decomposed into twoloops. The inner loop is responsible for applying generalized forces that accom-plish the motion objectives. The outer force control loop is responsible for man-aging the task space and internal forces within the system. The outer force controlloop adds on only squeeze generalized forces to those generated by the inner loop.Thus, the outer loop has no effect on the motion performance of the inner loop. Theforce control loop can be used to load-balance the forces to avoid saturating actua-tors, to ensure that sufficient forces are being applied in the task space needed fortool operation, or to maintain contact or grasp. Thus, the inner loop is responsiblefor controlling the motion degrees of freedom while the outer loop is responsiblefor controlling the force degrees of freedom in the system. The following exer-cise investigates the use of the X matrix for load balancing of actuator torques.

Exercise 11.2 Torque minimization using squeeze forces.Let W denote the weight matrix for a weighted vector 2-norm. Define X in (11.14)as X = GcW. For a generalized force T, show that (I−Pms)T is the generalizedforce that has minimumW-norm and the same move force component as T.

In other words, the above exercise shows that the Tmv move component of a gen-eralized force, T, is the generalized force with minimalW-norm that accomplishesthe same motion as T. Minimizing such norms can be desirable, since weightednorms are often measures of actuator power and stress loads. Thus, from a con-trol perspective, the motion control loop can generate a candidate generalized forcecommand T, which is then modified by the squeeze control loop by the amount−PmsT, to derive the minimum-norm command for the actuators.

11.2.2 Augmented Dynamics with Loop Constraints

So far, we have not made any particular assumptions on the nature of, or the phys-ical origin of, the closure constraints. In this section, we look at the importantcase where the constraints are due to motion constraints between nodes in a tree-topology system [152]. Such constraints are referred to as loop constraints. An ex-ample is illustrated in Fig. 11.1. Rigid inter-node constraints require the relativespatial velocity of the constrained nodes to be zero. More generally, these inter-node


internal loop

Fig. 11.1 Example of a system topology with an internal loop

constraints are defined by hinges that allow non-zero relative spatial velocities, butconstrain them to lie in a subspace defined by the hinge joint map matrices. Exam-ples of such constraints are:

1. A closure constraint between a node and the inertial frame. This constraint ischaracterized by an equation of the form

Qrelx Vx = 0

where Vx ∈R6 is the spatial velocity at the node xwhile Qrelx ∈Ra×6 (with a�6), is the constraint matrix. A rigid constraint, where the node x is not allowed tomove, corresponds to an identity Qrelx matrix.

2. A constraint on the relative spatial velocity of a pair of nodes, x and y, can bestated as

Qrel [Vx−Vy] = 0

The above can be restated as

QVnd = 0 where Q�=[

Qrelx , −Qrely

]

, Vnd�=

[

Vx

Vy

]

(11.16)

When the x and y nodes are constrained to rigidly follow each other, the Qrel

matrix is the identity matrix.

More generally, let us assume that there are a set of closure nodes on the system, andthe closed-chain constraints are constraints on the spatial velocities of these nodes.Let Vnd denote the spatial velocities of these constrained nodes. Then for somematrix Q, the closure constraints (holonomic or non-holonomic) can be expressed as

d(θ,t) = QVnd−U = QJθ−U = 0 (11.17)


with J = B∗φ∗H∗ (from (3.53)) denoting the velocity Jacobian matrix for theseconstraint nodes on the system. Using (11.3) we can identify Gc as

Gc = QJ = QB∗φ∗H∗ (11.18)

While the only fundamental requirement is that the spanning tree multibody sys-tem have an SKO model, for the sake of exposition, we have adopted the specificrigid multi-link operator notation in the operator expression for the Jacobian matrix.There is no loss in generality since the ideas developed here easily extend to generalSKO models using the approach from Chap. 9. This specific form for Gc allows usto simplify the expressions in Lemma 11.1, as described in the following lemma.For loop constraints, d in (11.7) can be expressed as

d(θ,t)11.18,11.4

= Qαfnd+ QVnd− U(t) (11.19)

Lemma 11.2 Forward dynamics with loop constraints.The generalized accelerations for a closed-chain system with loop constraints, givenby (11.18), is

θ = θf+ θδ (11.20)

where, the free generalized accelerations, θf, the correction generalized accelera-tions, θδ, and the Lagrange multipliers, λ, are given by

θf�= [I−HψK]∗D−1{T −Hψ[KT +Pa+b]

}−K∗ψ∗a (11.21a)

λ = −[QΛQ∗]−1 d (11.21b)

θδ�= [I−HψK]∗D−1HψBQ∗λ (11.21c)

with αfnd denotes the spatial accelerations of the constraint nodes for the free-dynamics solution of (11.21a), Λ = B∗ΩB is the operational space compliancematrix, and Ω = ψ∗H∗D−1ψH is the extended operational space compliance ma-trix, defined in (10.12).

Proof: The derivation of the expressions in (11.21) begins with the expressionsin (11.9). Firstly, (11.21a) follows directly from the expression for tree-topologygeneralized accelerations in (9.54).

Equation (11.21b) follows from

GcM−1G∗

c11.18,9.51

= QB∗φ∗H∗[I−HψK]∗D−1[I−HψK]HφBQ∗

9.45= QB∗ψ∗H∗D−1HψBQ∗ = QB∗ΩBQ∗ 10.12

= QΛQ∗


Using this in (11.9b) establishes (11.21b).For (11.21c), we have

M−1G∗c

11.18,9.51= [I−HψK]∗D−1[I−HψK]HφBQ∗

9.45= [I−HψK]∗D−1HψBQ∗

This lemma provides explicit operator expressions for the constraint forces andthe generalized accelerations. TheΛ operational space compliance matrix appearingin (11.21b), and the Ω extended operational space compliance matrix, were origi-nally encountered in the context of operational space dynamics in Chapter 10. The(U−Qαfnd) term in (11.21b) is a measure of the constraint violation from the freesystem dynamics solution in (11.21a). Indeed, with the correct node spatial accel-erations, αnd, the (U − Qαnd) vanishes. We can establish this by differentiating(11.17) to obtain:

QVnd+Qαnd = U(t) ⇒ U−Qαnd = 0

Thus, an intuitive interpretation of (11.21b) is that the constraint error spatial ac-celerations from the free-dynamics solution are used to derive the constraint forcesnecessary to nullify the errors. Once the constraint forces are available, (11.21c)uses them to correct the dynamics solution for the free system.

The spatial operator expressions in (11.21) can be implemented, and evaluated re-cursively, leading to substantial savings in computational cost over Algorithm 11.1.Algorithm 11.2 describes a complete solution to the constrained forward dynamicsalgorithm with loop constraints. The procedure is O(N), except for the step involv-ing the solution of the (11.21b) matrix equation. The computational complexity ofthis step can be cubic in the number of constraints.

References [17, 29] describe alternative recursive procedures for solving the for-ward dynamics of closed-chain systems. Featherstone [50] describes extensions ofthe tree DCA algorithm to systems with loop constraints. Other relevant referencesfor this topic are [23, 53, 89, 132, 187].

Exercise 11.3 Expression for YC with loop constraints.For systems with loop constraints, show that the operator form of YC in (11.11) isgiven by

YC = [I−HψK]∗D− 12[

I−HψBQ∗(QΛQ∗)−1QB∗ψ∗H∗]D− 12 [I−HψK]

That is,

YC = [I−HψK]∗D− 12[

I−b(b∗D−1b)−1b∗]

D− 12 [I−HψK] (11.22)

where b�= HψBQ∗.


Algorithm 11.2 Forward dynamics with loop constraintsThe steps for the solution of the augmented closed-chain dynamics with loop con-straints in Lemma 11.2 are listed below:

1. Solve for θf in (11.21a) using the O(N) AB forward dynamics Algorithm 9.5on page 182. This also results in the computation of the articulated body inertiaquantities and the αfnd node spatial accelerations required by (11.21b).

2. Use Algorithm 10.2 on page 194 to compute Λ. Then solve the (11.21b) matrixequation for the λ Lagrange multipliers.

3. Use Algorithm 7.3 on page 131 for an O(N) gather/scatter recursive implemen-tation of θδ = [I−HψK]∗D−1HψBQ∗λ to compute θδ with fc = −Q∗λ.

4. Use (11.20) to compute θ.

When there are no closed loops in the overall system, Q = 0, and so b = 0.Consequently, the middle term in (11.22) reduces to I, and the expression for YCreduces to the expression for M−1 in (9.51) for tree-topology systems.

11.2.3 Dual-Arm System Example

Now we consider a constrained dynamics example consisting of two manipulatorswhose end-effectors are rigidly attached to each other (for instance when hold-ing a rigid task object), as illustrated in Fig. 11.2. In this section, we explicitly

Original dualarm system

System with loopclosure cut

Loop cuta b

Fig. 11.2 Dual-arm system with rigid end-effectors constraint

work through the forward dynamics solution for this system using the augmentedapproach.


Imagine that the closed-chain is cut as shown on the right side of the figure,decomposing the system into a pair of serial-chains, and a single cut-edge constraint.Cutting the chain results in arms 1 and 2 with n1 and n2 links, respectively. Therigid constraint between the tips of arms 1 and 2 leads to the following constraintequations:

V1(0) = V2(0) =⇒ J1 θ1 = J2 θ2 or Q

[

V1(0)

V2(0)

]

= 0

α1(0) = α2(0) or Q

[

α1(0)

α2(0)

]

= 0 with Q�= [I6, −I6]

(11.23)

With (11.23), the dynamical behavior of arms 1 and 2 is given by

M1θ1 +C1 = T1 +J∗1 f1, M2θ+C2 = T2 +J∗2 f2 where

[

f1

f2

]

�= Q∗f(0) (11.24)

For arm 1,

θ1 = M−11 (T1 −C1)+M−1

1 J∗1 f1 = θ1f+ θ1δ (11.25)

where

θ1f�= M−1

1 (T1 −C1) = [I−HψK]∗D−1[I−HψK](T −C)

and θ1δ

�= M−1

1 J∗1 f1 = [I−HψK]∗D−1HψBf1 (11.26)

θ1f represents the “free” generalized acceleration, i.e., the generalized acceleration

that would exist if the tip of arm 1 were unconstrained, while θ1δ is the correction

joint acceleration for arm 1 due to the presence of the tip constraint force f1. θ1f can

be obtained using the recursive O(n1) AB forward dynamics algorithm, as can θ1δ,

once f(0) is determined. A similar story holds for arm 2.Since V1(0) = J1θ1,

α1(0) = V1(0) = J1 θ1 + J1 θ111.25= J1(θ1f+ θ

1δ)+ J1 θ1 (11.27)

It then follows from (11.26) and (11.27) that

α1(0)11.27= α1f(0)+J1θ

1δ, where α1f(0)

�= J1θ1f+ J1 θ1

11.26= α1f(0)+Λ1f1, where Λ1

�= J1M

−11 J∗1

(11.28)

Similarly,

α2(0) = α2f(0)+Λ2f2 where Λ2�= J2M

−12 J∗2 (11.29)

11.3 Projected Closed-Chain Dynamics 221

Then, from the constraint condition in (11.23), we have

f(0)11.28,11.29

= −Λ−1c Q

[

α2f(0)

α1f(0)

]

where Λc�= Q

(

Λ1 00 Λ2

)

Q∗ =Λ1 +Λ2

(11.30)

Λc is the inverse of the effective inertia of the closed chain system reflected to theend-effectors point of contact. As discussed in Chap. 10,Λ1 andΛ2 can be obtainedvia O(n1) and O(n2) recursive algorithms, respectively. Noting that the inversionof Λc ∈ R6×6 involves a flat cost independent of n1 and n2, we see that this is anO(n1 +n2) recursive algorithm for solving the forward dynamics of the dual-armsystem of Fig. 11.2. Extensions of the dual-arm formulation to the multi-arm case,and to one where the task object being manipulated has internal degrees of freedom,is discussed in [86, 87, 144].

11.3 Projected Closed-Chain Dynamics

In this section, we look at the alternative projected formulation of closed-chain dy-namics. In this formulation, the system dynamics is defined in terms of the actual(N−nc) velocity degrees of freedom in the constrained system. Using the minimalsubset of generalized velocities ensures that the resulting motions of the system arealways consistent with the constraints and do not drift off the constraint manifold.We begin by restating (11.6):

(

M G∗c

Gc 0

)[

θ

−λ

]

=

[

T −C

U

]

(11.31)

Since Gc has full row-rank nc, there exists a full column rank matrix Xc ∈RN×N−nc , whose columns span the N −nc-dimensional null-space of Gc, i.e.,

GcXc = 0 (11.32)

This implies that any generalized velocity vector, θ, satisfying the constraints canbe expressed as:

θ = θp+Xc θr (11.33)

where θp is a particular solution to the Gc θ = U equation, and θr ∈ RN−nc isan arbitrary vector. In other words, the (N−nc) dimensional θr vector in (11.33)parametrizes the N−nc dimensional subspace of generalized velocities that is con-sistent with the constraints. Thus, we can think of θr as the minimal, independentgeneralized velocity coordinates for the system. Equation (11.33) provides a way toobtain the full θ generalized velocity coordinates from θp. θp = 0 for systems for


catastatic systems, i.e., for systems with U = 0. Differentiating (11.33) with respectto time we have

θ= θp+Xc θr+ Xc θr = θp+Xc θr, where θp�= θp+ Xc θr (11.34)

Thus,U

11.6= Gc θ

11.34= Gc(θp+Xc θr)

11.32= Gc θp (11.35)

The top half of (11.31) is given by

T −C = Mθ−G∗cλ

11.34= M(θp+Xc θr)−G∗

cλ

Pre-multiplying this by X∗c, and using (11.32), leads to

Mr θr = X∗c(T −C−Mθp) where Mr

�= X∗

cMXc ∈RN−nc×N−nc (11.36)

Equation (11.36) represents the equations of motion projected onto the constrainedvelocity subspace. Mr represents the mass matrix in the projected dynamics for-mulation. It is symmetric, and also positive definite, because Xc is full-rank. Thesmaller dimension of Mr matches the true (N−nc) degrees of freedom in the sys-tem. The new θr generalized velocities are also of minimal (N−nc) dimension.

Observe that the λ constraint force Lagrange multipliers have been eliminatedfrom the equations of motion. Thus the projected dynamics formulation does notrequire the explicit computation of the λ Lagrange multipliers needed in the aug-mented dynamics formulation. Moreover, the projected equations of motion in(11.36) are in the form of an ordinary differential equations (ODE) instead ofthe more complex differential-algebraic equations for the augmented formulation.Mariti et al. [122] discusses the performance of several proposed methods for deter-mining the minimal independent coordinates for projected dynamics.

Algorithm 11.3 describes the O(N3) forward dynamics solution procedure forthe projected formulation. It requires the computation of the projected mass matrix,Mr, which can be expensive, since we need to solve for Xc. We also need to carryout the matrix products in (11.36). However, since the projected equations of motiondo not correspond to an SKO model, the techniques for factorizing and inverting themass matrix and algorithms such as the O(N) AB forward dynamics algorithm forSKO models cannot be used with the projected dynamics formulation.

11.4 Equivalence of Augmented and Projected Dynamics

We now show that the augmented and projected forms of the equations of motiondeveloped in Sects. 11.2.2 and 11.3, respectively, are indeed mathematically equiva-lent (even though the forward dynamics solution algorithms are very different). Thederivation here is an adaptation of that in Schwerin [158].

11.4 Equivalence of Augmented and Projected Dynamics 223

Algorithm 11.3 Forward dynamics for projected closed-chain dynamicsThe forward dynamics for the projected formulation of the equations of motionconsist of the following steps:

1. Decompose the closed-chain system into a spanning tree with cut-edge con-straints, and defineGc for the system.

2. Solve (11.32) for Xc and (11.33) for θp.3. Compute the M mass matrix for the tree system using Algorithm 9.3.4. Compute Mr using (11.36).5. Solve (11.36) for θr.6. Integrate these accelerations to obtain θr.7. Use θr in (11.33) to obtain the θ generalized velocities for the system.

Since Gc has full row-rank, there exists an invertible transformation P ∈ RN×N

such thatG

�= Gc P = [Gr, 0] ∈Rnc×N (11.37)

where Gr ∈Rnc×nc is square and non-singular. With

Xc�=

[

0I

]

∈RN×N−nc , G Xc11.37= 0 ⇒ Gc P Xc

11.37= 0 (11.38)

It follows thatXc

11.38= P Xc (11.39)

satisfies the GcXc = 0 condition in (11.32).

Exercise 11.4 Explicit expression for P.Assuming thatGc1 ∈Rnc×nc is non-singular in the block-partitioned expressionGc = [Gc1, Gc2], show that an example of P in (11.37) is

P =

(

I −G−1c1 Gc2

0 I

)

, with Gr =Gc1 and Xc =

[

−G−1c1 Gc2

I

]

(11.40)

The following exercise shows that the augmented dynamics can be partitionedusing the P transformation matrix from (11.41).

Exercise 11.5 Transformed and partitioned augmented dynamics.Use the P transform to define a new transformed generalized accelerations vector,θ, a generalized forces vector, T, and a mass matrix, M, as

θ�= P−1 θ ∈RN, T

�= P∗(T −C) ∈RN, M

�= P∗MP ∈RN×N (11.41)


Show that the (11.6) augmented equations of motion, in the transformed coordi-nates, can be expressed in the following equivalent block-partitioned form:

⎛

⎜

⎝

M11 M12 G∗r

M21 M22 0Gr 0 0

⎞

⎟

⎠

⎡

⎢

⎣

θ1

θ2

−λ

⎤

⎥

⎦=

⎡

⎢

⎣

T1

T2

U

⎤

⎥

⎦(11.42)

where the sub-blocks size is M11 ∈ Rnc×nc , M22 ∈ RN−nc×N−nc , θ1 ∈ Rnc ,θ2 ∈RN−nc , etc.

The following exercise uses the partitioned structure of the transformed equationsof motion in Exercise 11.5 to develop an equivalent, reduced version of the equationsof motion.

Exercise 11.6 Reduction of augmented dynamics.Show that the augmented closed-chain dynamics equation in (11.6) is equivalent tothe following reduced equations of motion:

M22 θ2 =(

T2 −M21G−1r U

)

(11.43)

The following exercise shows that the projected equations of motion in (11.36)are equivalent to the reduced version of the augmented equations of motion in(11.43).

Exercise 11.7 Transformed projected dynamics.Show that the projected equations of motion in (11.36) are equivalent to the reduced,augmented equations of motion in (11.43).

Together, Exercises 11.6 and 11.7 establish the equivalency of the augmentedand projected formulations of closed-chain system dynamics.

11.5 Unilateral Constraints

Rigid body-to-body contact and joint limits are examples of unilateral constraints.The traditional approaches for handling such constraints treat them as soft con-straints and make use of penalty methods [51]. In the case of body-to-body contact,penalty methods permit the inter-penetration of bodies, and generate resisting forcesthat are a function of the penetration distance, using non-linear spring/damper mod-els. While penalty methods provide continuous detail of the impact and contact pro-cesses, they can require small integration time step sizes during the contact events

11.5 Unilateral Constraints 225

due to the stiffness of the contact models. In this section, we briefly describe alterna-tive methods for handling unilateral constraints that are known as complementaritymethods. Unlike penalty methods, these methods do not attempt to continuouslytrack the state of the unilateral constraints.

Instead of the equality relationship in (11.2) for bilateral constraints, unilateralconstraints are defined by an inequality relationship such as:

d(θ,t) � 0 (11.44)

For example, the non-penetration condition for rigid bodies translates into such asinequality relationship on the distance between the bodies. This constraint requiresthat the distance between the surfaces of rigid bodies be non-negative. d(θ,t) is thuscommonly referred to as the distance or gap function for unilateral constraints.

Contact occurs at the constraint boundary, i.e., when d(θ,t) = 0. For bodies incontact, the surface normals at the contact point are parallel. The existence of contactis often determined by geometric analysis, such as collision detection techniques.During contact, the relative linear velocity along the normals at the point of contactis zero as well. A unilateral constraint is said to be in an active state when

d(θ,t) = d(θ,t) = d(θ,t) = 0 (11.45)

Thus, a unilateral constraint is active when there is contact, and the contact persists.Only active constraints generate constraint forces on the system.

The constraint that is not active is said to be inactive. Clearly, constraints notin contact, are inactive. Contact separation occurs when the relative linear velocityof the contact points along the normals becomes positive and the contact pointsdrift apart. A separating constraint is in the process of losing contact and marks thebeginning of its inactive state. At the start of a separation event, we have

d(θ,t) = d(θ,t) = 0 and d(θ,t)> 0 (11.46)

11.5.1 Complementarity Problems

From (11.5) it follows that

θ= M−1 [T −C+G∗c λ]

11.9a= θf+M−1G∗

c λ (11.47)

Thus

d11.7,11.47

= Gc θf−U+GcM−1G∗

c λ11.9b= df+GcM

−1G∗c λ (11.48)

This provides an expression for the d gap accelerations in terms of the df gap ac-celerations that arise in the absence of unilateral constraints, and the unknown con-straint forces λ that result from the active unilateral constraints.


When all the unilateral constraints are active, d = 0, and (11.48) can be solved toobtain the λ constraint forces, and subsequently used in (11.47) to obtain the gener-alized accelerations for the system. For this case, the forward dynamics solution forthe system is precisely that given by the augmented dynamics solution in (11.9) forbilateral constraints. The same story applies in the other extreme when all the unilat-eral constraints are inactive. Since inactive constraints generate no constraint forces,the augmented forward dynamics solution applies with these constraints removed.In general, however, some unilateral constraints are active and some are inactive. Ifthe states of the constraints are known, then the forward dynamics procedure for bi-lateral constraints can be directly applied with only the active unilateral constraintsincluded. The primary challenge with unilateral constraints is the determination andtracking of the changing active and inactive states of the constraints as the dynamicsof the system evolves.

To illustrate this procedure, consider the example of friction-less contact betweenrigid bodies. When the kth such constraint is active, d(k) = 0, and the λ(k) con-straint force is non-zero and positive. On the other hand, when the constraint isseparating, d(k) > 0, and the constraint force λ(k) = 0. These dual relationshipscan be formally stated as

0 � d ⊥ λ � 0 (11.49)

The above relationship is referred to as a complementarity condition. The ⊥ in(11.49) implies that only one of the acceleration or force values can be non-zerofor any component of the λ and d vectors, depending on whether the correspondingconstraint is active or not. This condition is equivalent to

d � 0, λ � 0, and λ∗ d = 0

Thus, for unilateral constraints, (11.48) needs to be solved subject to the comple-mentarity condition in (11.49). Problems involving such complementarity condi-tions are referred to as nonlinear complementarity problems (NCP). In general, anNCP problem requires solving for unknown vectors w and z that are subject tocomplementarity conditions such as:

0 � w(z) ⊥ z � 0 (11.50)

The solution of the complementarity problem identifies the active contact con-straints, as well as the λ constraint forces for the active ones. The λ constraintforces can be used in (11.47) to obtain the full generalized accelerations for thesystem. Observe that the GcM−1G∗

c matrix in (11.46) is invertible only if Gc(θ,t)is full rank, i.e., if the unilateral constraints are independent. Such independencecan be lost in the case of multiple contact constraints. Other types of unilateral con-straints can also be cast into appropriate complementarity conditions on the system[31, 136, 174, 175].

A linear complementarity problems (LCP) [40] is a special type of NCP, wherea matrix F and vector f are known, and a solution for the unknown vectorsw and zsatisfying the following is required:

11.5 Unilateral Constraints 227

w= Fz+ f where 0 � w⊥ z � 0 (11.51)

Since an LCP is easier to solve compared to an NCP, it is advantageous to formulatea unilateral constraint problem as an LCP. While this occurs naturally for sometypes of unilateral constraints, techniques such as, polyhedral approximations of thefriction cone for contacts with friction, are often used to transform an underlyingNCP into an LCP [175].

11.5.2 Forward Dynamics

A common variation of the complementarity formulation is to directly incorporatetime-stepping into the formulation, to facilitate the solution process [131, 188]. Inthis formulation, the complementarity problem solves for the system state at theend of the time step and works with impulses instead of forces over the time stepinterval.

In addition, it is common to see formulations that cast bilateral constraints in theform of complementarity conditions. This is not recommended, since it increasesthe dimensionality and cost of the LCP solution, and leads to drift issues that arisefrom the over parametrization of bilateral constraints. The formulation describedhere, on the other hand, allows the normal handling of bilateral constraints so thatthe complementarity techniques are limited to the unilateral constraints.

Since the LCP solution can be computationally expensive, reducing the need forits solution is desirable. Featherstone [51] suggests a forward dynamics strategybased on the observation that the LCP solution is not required if the active andinactive states of the constraints are already known, since the standard augmenteddynamics equations can be solved with just the active constraints included. Thestrategy consists of solving the forward dynamics problem assuming that there is nochange in the active and inactive states of the unilateral constraints. The solution ischecked to verify that it does not violate the (11.49) complementarity conditions. Aviolation implies that the state of a constraint is changing. In this case, the solutionis discarded, and the LCP is solved to reevaluate the set of active constraints beforeproceeding with the solution.

Chapter 12Systems with Geared Links

So far, we have studied the dynamics modeling of systems with non-geared,direct-drive hinge actuators. However, many systems have geared joints. Whenthe gear ratios are large, the motor inertias have significant effect on the systemdynamics, since the equations of motion contain motor inertia terms that are mag-nified by the square of the gear ratio. For such systems, it is important that gear andactuator dynamics be included in the dynamics model of the manipulator. Relianceon simplified and approximate models can lead to errors in control design, withconsequent degradation in system stability and performance.

A useful approximation [12, 135] for handling geared actuators adds the pro-jection of the amplified rotational inertias of the actuator motors to the diago-nal elements of the manipulator mass matrix (formed by ignoring the actuators).This model ignores gyroscopic effects arising from the high spin rates of the motor.To handle geared actuator effects, various assumptions have been used to sim-plify and decouple the actuator dynamics from the dynamics of the manipulator[170]. These approximations, while useful in many cases, have the followingdisadvantages:

1. The modified mass matrix must still be inverted (or, alternatively, a linear systemof equations must be solved) to solve the forward dynamics problem.

2. The powerful direct-drive computational algorithms [120, 179], cannot be used.3. Coupling terms in the mass matrix, gyroscopic forces arising from the spinning

of the motors, that significantly alter the behavior of the system are not fullyaccounted for [36, 37, 77, 126, 127].

In this chapter, we develop an SKO dynamics formulation for geared systems thatmodels the geared effects correctly, and derive efficient SKO algorithms for thesystem.


230 12 Systems with Geared Links

12.1 Equations of Motion

One degree-of-freedom rotary joints are assumed, with the joint motors mounted onthe inboard link. A typical link, with a motor driving the outboard link through a setof gears, is shown in Fig. 12.1. We assume that the system is a canonical serial-chain

Ok

Ok−1

kth

hinge

(k−1)th

hinge

kth link

(k + 1)th

link

(k−1)th link

(k−1)th motor


pl(k)

pm(k−1)

ll(k,k−1)

lm(k,k−1)

Omk−1

hl(k)

hl(k−1)

hm(k−1)

Fig. 12.1 Link with a geared motor mounted on it

system with n geared rigid-body links. Each driven link has associated with it amotor body. Thus, there are 2n bodies in the system. For each such link/motor pairof bodies, we refer to the driven body with the “l” subscript, and the motor bodywith the “m” subscript. We number the link/motor pairs with indices proceedingfrom tip-to-base in increasing order from 1 to n.

A key difference between geared and non-geared dynamics is the presence of akinematic closed-loop at each of the hinges. Thus, the underlying digraph for thesystem, illustrated on the left in Fig. 12.2, is not a tree. Instead of using the con-strained dynamics techniques from Chap. 11, we capitalize on the simple nature ofthe loops to develop a projected formulation, i.e., one with minimal generalizedvelocity coordinates that retain the SKO structure of the equations of motion. Thedevelopment in this chapter also sets the stage for the more general constraint em-bedding technique described in Chap. 15 for non-tree-topology multibody systems.

While the only fundamental requirement is that the spanning tree for the gearedsystem have an SKO model, for the sake of simpler exposition, we have focusedhere instead on a serial-chain, rigid-link geared system. There is no loss in generality

12.1 Equations of Motion 231

(k+1)th link

kth linkkth motor

(k−1)th motor

Fig. 12.2 The figure on the left illustrates the original non-tree digraph associated withgeared links systems. The figure on right is the spanning tree digraph for the system

since the ideas developed here easily extend to general spanning tree SKO modelsusing the approach from Chap. 9.

We begin by ignoring the gearing constraints, and describe the equations of mo-tion for the spanning tree system illustrated on the right in Fig. 12.2.

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

V(n+ 1) = α(n+ 1) = 0

for k = n · · ·1Vm(k) = φ∗

m(k+ 1,k)Vl(k+ 1)+H∗m(k)θm(k)

Vl(k) = φ∗l (k+ 1,k)Vl(k+ 1)+H∗

l (k)θl(k)

αm(k) = φ∗m(k+ 1,k)αl(k+ 1)+H∗

m(k)θm(k)+am(k)

α(k) = φ∗l (k+ 1,k)αl(k+ 1)+H∗

l (k)θl(k)+al(k)

end loop

(12.1)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

fm(0) = 0, fl(0) = 0

for k = 1 · · ·nfm(k) =Mm(k)αm(k)+bm(k)

fl(k) = φl(k,k− 1)fl(k− 1)+Ml(k)αl(k)

+φm(k,k− 1)fm(k− 1)+bl(k)

Tm(k) =Hm(k)fm(k)

Tl(k) =Hl(k)fl(k)

end loop

The equations of motion in (12.1) are identical to the SKO versions for tree systemsdescribed in (9.10) on page 166. The only difference in form is that we have ex-plicitly listed the pair of equations for the two bodies in each link/motor pair. Thespatial velocity Vl(k) of the kth link at the Ok frame is obtained by propagating


the spatial velocity Vl(k+ 1) of the inboard link to Ok, and then adding the relativespatial velocity at the kth joint. The spatial velocity Vm(k) of the Omk frame on thekth motor is obtained similarly. Acceleration equations for αl(k) and αm(k), ob-tained by differentiating velocity equations, contain the al(k) and am(k) Coriolisacceleration terms. There are two force balance equations. The first involves thespatial force fm(k) due to motor motion. The second involves the forces fl(k) andfl−1(k− 1) due to the outboard and inboard links, the force fm(k− 1) due to mo-tor motion, the D’Alembert force Ml(k)αl(k) due to the kth link itself, and thegyroscopic force bl(k).

12.1.1 Reformulated Equations of Motion

Define the following quantities:

θ(k)�=

[

θm(k)

θl(k)

]

, a(k)�=

[

am(k)

al(k)

]

, b(k)�=

[

bm(k)

bl(k)

]

ΦG(k+ 1,k)�=

(

0 0φm(k+ 1,k) φl(k+ 1,k)

)

∈R12×12

H(k)�=

(

Hm(k) 0

0 Hl(k)

)

∈R2×12

M(k)�=

(

Mm(k) 06

06 Ml(k)

)

∈R12×12

(12.2)

Using these, (12.1) can be rewritten in the following more compact form

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

V(n+ 1) = α(n+ 1) = 0

for k = n · · ·1

V(k)�=

[

Vm(k)

Vl(k)

]

=Φ∗G(k+ 1,k)V(k+ 1)+H∗(k)θ(k)

α(k)�=

[

αm(k)

αl(k)

]

=Φ∗G(k+ 1,k)α(k+ 1)+H∗(k)θ(k)+a(k)

end loop

12.1 Equations of Motion 233

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

f(0) = 0

for k = 1 · · ·n

f(k)�=

[

fm(k)

fl(k)

]

=ΦG(k,k− 1)f(k− 1)+M(k)α(k)+b(k)

T(k)�=

[

Tm(k)

Tl(k)

]

=H(k)f(k)

end loop

(12.3)

Observe thatΦG(k+ 1,k) can be factored as follows:

ΦG(k+ 1,k)�= BGAG(k+ 1,k) (12.4)

where AG(k+ 1,k)�= [φm(k+ 1,k), φl(k+ 1,k)] ∈R6×12

and BG�=

[

06

I6

]

∈R12×6(12.5)

At this stage, the equations of motion are in an SKO model form for the systemspanning tree with the ΦG(k+ 1,k) terms defining the SKO weight matrices. Thisform of the SKO model is suitable for modeling systems with flexible joints, i.e.,systems whose hinge actuators have internal dynamics and states [77, 127]. Thenative tree-topology of these systems allows the use of SKO model techniques forfurther analysis and algorithm development.

On the other hand, geared actuators impose in an additional holonomic constraintbetween the hinge and the driven link generalized coordinates. The following sec-tion discusses steps that can be taken to eliminate these constraints to obtain an SKOmodel for the geared system.

12.1.2 Eliminating the Geared Constraint

The holonomic geared constraint enforces the condition that the link hinge general-ized coordinate is related to the motor generalized coordinate at the kth hinge byits gear ratio as follows:

θm(k) = μG(k)θl(k), i.e., θ(k) =

[

μG(k)

I

]

θl(k) (12.6)

where μG(k) is the gear ratio for the kth hinge. Due to this constraint, θl(k)and θm(k) are not independent. Either one can be chosen as generalized coordi-nates for the kth hinge. Choosing θl(k) as the independent generalized coordinate,the principle of virtual work states that the corresponding generalized forces are[μG(k), I]T(k) = μG(k)Tm(k)+Tl(k).


The geared constraint can be eliminated by simply redefining T(k), and definingHG(k) as follows:

T = μG(k)Tm(k)+Tl(k)

HG(k) =[

μG(k), I]

H(k)12.2=[

μG(k)Hm(k), Hl(k)]

(12.7)

With these definitions, and replacing H(k) with HG(k), (12.3) is transformed into

the following equations of motion for the system:

⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

V(n+ 1) = α(n+ 1) = 0

for k = n · · ·1V(k) =Φ∗

G(k+ 1,k)V(k+ 1)+H∗G(k)θl(k)

α(k) =Φ∗G(k+ 1,k)α(k+ 1)+H∗

G(k)θl(k)+a(k)

end loop

(12.8)⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

f(0) = 0

for k = 1 · · ·nf(k) =ΦG(k,k− 1)f(k− 1)+M(k)α(k)+b(k)

T(k) =HG(k)f(k)

end loop

Now θl(k) ∈R1 and T(k) =HG(k)f(k) ∈R1 are the generalized coordinates andforces, respectively, for the kth hinge. They automatically enforce the geared con-straints, and, as a consequence, the explicit geared constraints have been eliminated!Thus, we have in effect transformed the original non-tree digraph with 2n nodes intoa tree digraph with just n nodes as illustrated in Fig. 12.3.

12.2 SKO Model for Geared Systems

While the (12.8) formulation has only n independent θl and θl generalizedvelocities and accelerations instead of the 2n values in (12.1), it also has a formidentical to the SKO formulation for tree systems. This allows us to proceed withan SKO formulation of the system dynamics together with the definitions of the as-sociated operators. One difference is that V(k), α(k), f(k), a(k) and b(k) are all12-dimensional vectors, as opposed to the 6-dimensional vectors that we haveencountered previously.

12.2 SKO Model for Geared Systems 235

(k+1)th

link(k+1)th

link

kth link

kth

motorcombined kth

link & motornode

Original closed-graph topology Transformed tree topology

Fig. 12.3 Illustration of the non-tree digraph associated with the original system andthe transformed tree-digraph after the kth link/motor body nodes are combined intoa single node. Only the kth link/motor combined node is shown in the figure

Define the stacked vectors

V =

⎡

⎢

⎢

⎣

V(1)...

V(n)

⎤

⎥

⎥

⎦

, α=

⎡

⎢

⎢

⎣

α(1)...

α(n)

⎤

⎥

⎥

⎦

, f =

⎡

⎢

⎢

⎣

f(1)...

f(n)

⎤

⎥

⎥

⎦

, T =

⎡

⎢

⎢

⎣

T(1)...

T(n)

⎤

⎥

⎥

⎦

and, similarly, the stacked vectors a and b. Then, (12.2) can be restated as:

V =Φ∗GH

∗G θl

α=Φ∗G[H∗

G θl+a]

f =ΦG[Mα+b]

T =HGf

(12.9)

where HG�= diag

{HG(1), · · · HG(n)

}nk=1

, M�= diag

{M(1), · · · M(n)

}nk=1

,

and the EΦG and ΦG SKO and SPO operators, respectively, for the system aredefined as:

EΦG�=

n−1∑k=1

e℘(k)ΦG(℘(k),k)e∗k and ΦG�= (I−EΦG)−1 (12.10)

Based on the velocity relationships in (12.8),ΦG(j,k) is the (j,k) weight matrix forthe EΦG SKO operator. The weights satisfy the usual semi-group property:

ΦG(k, j)�= ΦG(k,k− 1) · · ·ΦG(j+ 1, j) for k > j

It is noteworthy that theΦG(k, j) weight matrices are neither invertible nor are they6×6 matrices, as has been the case for rigid-link systems with non-geared hinges.


Moreover, the weight matrices are of non-uniform size for systems that have a mixof links with geared and non-geared hinges.

With our canonical serial-chain assumption for the system, EΦG and ΦG havethe following form:

EΦG =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0

ΦG(2,1) 0 . . . 0 0

0 ΦG(3,2) . . . 0 0...

.... . .

......

0 0 . . . ΦG(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

ΦG =

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0

ΦG(2,1) I . . . 0...

.... . .

...

ΦG(n,1) ΦG(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

.

12.2.1 Expression for the Mass Matrix

Together, the expressions in (12.9) lead to the following equations of motion:

T = MG θl+CG (12.11)

where the mass matrix MG is

MG�= HGΦGMΦ∗

GH∗G (12.12)

and the vector of Coriolis and gyroscopic generalized forces CG is

CG�= HGΦG [b+MΦ∗

Ga] (12.13)

From (12.12), it is clear that the mass matrix MG is symmetric. It is positive defi-nite because M is positive definite. The expression in (12.12) is the Newton–EulerOperator Factorization of the mass matrix.

The (HG,ΦG,M) operators satisfy the conditions in Sect. 9.1.1 for an SKOmodel Hence, we can directly apply the SKO techniques developed in Chap. 9 fordecomposing, factoring and inverting the mass matrix for the geared systems. How-ever, there are differences in the internal structure of the operators which result indifferences in the component steps, and also allow for further optimization of theSKO algorithms.

12.3 Computation of the Mass Matrix 237

12.3 Computation of the Mass Matrix

We now discuss the algorithm for computing the exact mass matrix for gearedsystems based on the SKO procedures from Sect. 9.3.2. First, we adapt Algorithm9.2 on page 169 to derive the recursive Algorithm 12.1 for the RG(k) ∈ R12×12

composite body inertias.

Algorithm 12.1 Composite rigid body inertias⎧⎪⎪⎪⎨⎪⎪⎪⎩

RG(0) = 0

for k = 1 · · ·nRG(k) =ΦG(k+ 1,k)RG(k− 1)Φ∗

G(k+ 1,k)+M(k)

end loop

(12.14)

With the block-diagonal operator RG�= diag

{RG(k)

}nk=1

we can now define the

operator decomposition in the following lemma.

Lemma 12.1 Decomposition of ΦGMΦ∗G.

ΦGMΦ∗G = RG+ ΦGRG+RGΦ

∗G (12.15)

where ΦG�= ΦG− I.

Proof: See Lemma 9.4.Substituting (12.15) in (12.12), the mass matrix MG can be decomposed as

follows:MG =HG

[

RG+ ΦGRG+RGΦ∗G

]

H∗G (12.16)

As seen in Lemma 9.4, the first term is block diagonal; the second is strictly upper-triangular, and the last is strictly lower-triangular. Paralleling Algorithm 9.3, thisdecomposition leads to the following recursive algorithm for computing the massmatrix.

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

RG(0) = 0

for k = 1 · · ·nRG(k) =ΦG(k,k− 1)RG(k− 1)Φ∗

G(k,k− 1)+M(k)

MG(k,k) =HG(k)RG(k)H∗G(k)⎧⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎩

X(k+ 1) =ΦG(k+ 1,k)RG(k)H∗G(k)

for j = k+ 1 · · ·nMG(j,k) = M∗

G(k, j) =HG(j)X(j)

X(j+ 1) =ΦG(j+ 1, j)X(j)

end loop

end loop

(12.17)


12.3.1 Optimized Composite Body Inertia Algorithm

Due to the sparse structure of ΦG(k+ 1,k), further simplifications are possible.These are discussed in the following exercise.

Exercise 12.1 Simplification of RG(k).Show that RG(k) and X(j) in (12.17) have the following partitioned structure

RG(k) =

(

Mm(k) 0

0 RGl(k)

)

, X(j) =

[

0

Xl(j)

]

(12.18)

where RGl(k) ∈R6×6 and Xl(k) are defined recursively as:

RGl(k) = φl(k,k− 1)RGl(k− 1)φ∗l (k,k− 1)+M(k)

Xl(k+ 1) = Lm(k+ 1)+φl(k+ 1,k)RGl(k)H∗l (k)

Xl(j+ 1) = φl(j+ 1, j)Xl(j)

MG(k,k) = Dm(k)+Hl(k)Xl(k)

(12.19)

and

M(k)�= φm(k,k− 1)Mm(k− 1)φ∗

m(k,k− 1)+Ml(k)

Dm(k)�= μ2

GHm(k)Mm(k)H∗m(k)

Lm(k)�= μGφm(k+ 1,k)Mm(k)Hm(k)

(12.20)

Physically, M(k) is the spatial inertia about Ok of a composite rigid bodyconsisting of the kth link and the (k− 1)th motor. Using (12.18)–(12.20), thealgorithm in (12.17) can be simplified into Algorithm 12.2. Because the quantitiesM(k), Dm(k) and Lm(k) are all constant in the local link frame, they need to becomputed just once.

The transformation of (12.17) into the improved Algorithm 12.2 is an exam-ple of the optimization step in the SKO formulation step described in Sect. 9.6 onpage 182, where the system specific structure of the SKO and other operators is usedto further optimize and improve upon the standard SKO algorithm.

An approximate way of handling gears [12, 135] is to add the projection ofthe motor rotational inertia to the diagonal elements of the manipulator massmatrix (without the motors), and to ignore the additional gyroscopic terms from thespinning of the motors. In particular, this approximate model ignores off-diagonalcross-coupling terms in the mass matrix. Setting φm(k,k− 1) = 0 in (12.17) leadsto this approximate mass matrix. However, this approximation is unnecessary,since the exact mass matrix can be obtained with little additional overhead usingAlgorithm 12.2.

12.4 O(N) AB Forward Dynamics 239

Algorithm 12.2 The mass matrix for geared hinge systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

RG(0) = 0

for k = 1 · · ·nRGl(k) = φl(k,k− 1)RGl(k− 1)φ∗

l (k,k− 1)+M(k)

Xl(k) = RGl(k)Hl∗(k)

MG(k,k) = Dm(k)+Hl(k)Xl(k)⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

Xl(k+ 1) = Lm(k+ 1)+φl(k+ 1,k)RGl(k)H∗l (k)

for j = k+ 1 · · ·nMG(j,k) = M∗

G(k, j) =Hl(j)Xl(j)

Xl(j+ 1) = φl(j+ 1, j)Xl(j)

end loop

end loop

12.4 O(N) AB Forward Dynamics

We now proceed to apply the SKO formulation based O(N) AB forward dynamicsalgorithms from Sect. 9.5.1 on page 180 to the geared system.

12.4.1 Mass Matrix Factorization and Inversion

First, define the quantities PG(k),DG(k),GG(k),KG(k+ 1,k) and ΨG(k+ 1,k)via the recursive Algorithm 12.3 which is derived from (9.33).

Define the block-diagonal operators

PG�= diag

{PG(k)

}nk=1

, DG�= diag

{DG(k)

}nk=1

, and GG�= diag

{GG(k)

}nk=1

along the lines of (9.32). The operators EΨG and K are defined in the same manneras EΦG in (12.10) except that the entries are ΨG(k+ 1,k)s and K(k+ 1,k) ratherthan the ΦG(k+ 1,k) terms. As is the case for EΦG , EΨG is also nilpotent, so theoperator ΨG can be defined as:

ΨG�= (I−EΨG)−1 =

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0

ΨG(2,1) I . . . 0...

.... . .

...

ΨG(n,1) ΨG(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

(12.22)


Algorithm 12.3 Standard articulated body inertia algorithm for geared hingesystems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

PG(0) = 0,DG(0) = 0

for k = 1 · · ·nPG(k) =ΦG(k,k− 1)PG(k− 1)Φ∗

G(k,k− 1)−

KG(k,k− 1)DG(k− 1)K∗G(k,k− 1)+M(k)

DG(k) =HG(k)PG(k)H∗G(k)

GG(k) = PG(k)H∗G(k)D−1

G (k)

KG(k+ 1,k) =ΦG(k+ 1,k)GG(k)

ΨG(k+ 1,k) =ΦG(k+ 1,k)−KG(k+ 1,k)HG(k)

end loop

(12.21)

where

ΨG(k, j)�= ΨG(k,k− 1) · · ·ΨG(j+ 1, j) for k > j

Using these spatial operators, we can rewrite (12.21) in stacked notation as

PG = EΦGPGE∗ΦG

−KGDGK∗G+M

DG =HGPGH∗G

GG = PGH∗GD−1

G

KG = EΦGGG

EΨG = EΦG −KGHG

(12.23)

The next lemma describes the familiar Innovations Operator Factorization and in-

version of the mass matrix, and the operator expression for the θl generalized ac-celerations in terms of the T generalized forces.

Lemma 12.2 Mass matrix factorization and inversion for gearedsystems.

MG = [I+HGΦGKG]DG[I+HGΦGKG]∗

[I+HGΦGKG]−1 = [I−HGΨGKG]

M−1G = [I−HGΨGKG]∗D−1

G [I−HGΨGKG]

θl = [I−HGΨGKG]∗D−1G

{T −HGΨG

∗ [KGT +b+PGa]}

−K∗GΨG

∗a

(12.24)

Proof: See Lemmas 9.11 and 9.12.


12.4.2 Recursive AB Forward Dynamics Algorithm

Along the lines of Algorithm 9.5 on page 182, we use (12.24) to obtain the recursiveAB forward dynamics Algorithm 12.4 for computing θl.

Algorithm 12.4 Forward dynamics for systems with hinge flexibility⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

z(0) = 0, T(0) = 0

for k = 1 · · ·nz(k) =ΦG(k,k− 1)z+(k− 1)+b(k)+PG(k)a(k)

ε(k) = T(k)−HG(k)z(k)

ν(k) = D−1G (k)ε(k)

z+(k) = z(k)+GG(k)ε(k)

end loop

(12.25)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

α(n+ 1) = 0

for k = n · · ·1α+(k) =Φ∗

G(k+ 1,k)α(k+ 1)

θl(k) = ν(k)−G∗G(k)α+(k+ 1)

α(k) = α+(k)+H∗G(k)θl(k)+a(k)

end loop

The steps in the algorithm can be summarized as follows:

1. Carry out the base-to-tip recursion sweep in (12.1) to compute the spatial veloc-ities Vm(k) and Vl(k) for all links. The a(k) Coriolis spatial accelerations andb(k) gyroscopic spatial forces are also computed.

2. Carry out the tip-to-base sweep in (12.21) to compute the PG(k), DG(k), GG(k)and KG(k) articulated body inertia terms for all the links.

3. Carry out the tip-to-base recursion followed by the base-to-tip recursion in(12.25) to compute the generalized accelerations θl(k).

The tip-to-base recursion in step 2 can be combined with the first tip-to-base recur-sion in step 3.


12.4.3 Optimization of the Forward Dynamics Algorithm

The forward dynamics algorithm described in Sect. 12.4.2 can be further optimizedusing the sparsity of several of the quantities involved, and is the subject of thefollowing exercise.

Exercise 12.2 Structure of PG(k) and related quantities.Use the sparsity ofΦG(k+ 1,k) and the partitioning ofHG(k) in (12.20) to show

that the quantities in (12.21) have the following form:

PG(k) =

(

Mm(k) 0

0 Pl(k)

)

DG(k) = Dm(k)+Hl(k)Pl(k)H∗l (k)

GG(k) =

[

μGMm(k)Hm(k)D−1G (k)

Gl(k)

]

KG(k+ 1,k) =

[

0

K(k+ 1,k)

]

(12.26)

where the terms Pl(k) ∈ R6×6, Gl(k) and K(k+ 1,k) satisfy the following recur-

sive relationships:

Pl(k)�= φl(k,k− 1)Pl(k− 1)φ∗

l (k,k− 1)

−K(k,k− 1)DG(k− 1)K∗(k,k− 1)+M(k)

Gl(k)�= Pl(k)Hl(k)D

−1G (k)

K(k+ 1,k)�= Lm(k+ 1)D−1

G (k)+φl(k+ 1,k)Gl(k)

Furthermore, show that the steps in (12.25) for computing z(k) and α+(k) can berestated as

z(k) =

[

Mm(k)am(k)+bm(k)

AG(k,k− 1)z+(k− 1)+Pl(k)al(k)+bl(k)

]

α+(k) = A∗G(k+ 1,k)αl(k+ 1)

Observe that the Pl(k) matrices are obtained recursively as a solution to a Ric-cati equation, similar to the one for direct-drive manipulators. These new expres-sions can be used to simplify the O(N) AB forward dynamics Algorithm 12.4 toobtain the optimized Algorithm 12.5. This algorithmic optimization is an additionalexample of the SKO formulation optimization step described in Sect. 9.6. The com-


putational cost of thisO(N) forward dynamics algorithm for geared systems is onlyslightly more than that for theO(N) AB forward dynamics algorithm for non-gearedsystems.

Algorithm 12.5 Simplified forward dynamics for systems with hinge flexibility

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

Pl(0) = 0,DG(0) = 0

for k = 1 · · ·nPl(k)

�= φl(k,k− 1)Pl(k− 1)φ∗

l (k,k− 1)

−K(k,k− 1)DG(k− 1)K∗(k,k− 1)+M(k)

DG(k) = Dm(k)+Hl(k)Pl(k)H∗l (k)

Gl(k) = Pl(k)Hl(k)D−1G (k)

K(k+ 1,k) = Lm(k+ 1)D−1G (k)+φl(k+ 1,k)Gl(k)

GG(k) =

[

μGMm(k)Hm(k)D−1G (k)

Gl(k)

]

z(k) =

[

Mm(k)am(k)+bm(k)

AG(k,k− 1)z+(k− 1)+Pl(k)al(k)+bl(k)

]

ε(k) = T(k)−HG(k)z(k)

ν(k) = D−1G (k)ε(k)

z+(k) = z(k)+GG(k)ε(k)

end loop

(12.27)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

αl(n+ 1) = 0

for k = n · · ·1α+(k) = A∗

G(k+ 1,k)αl(k+ 1)

θl(k) = ν(k)−G∗G(k)α+(k+ 1)

α(k) = α+(k)+H∗G(k)θl(k)+a(k)

end loop

Chapter 13Systems with Link Flexibility

This chapter uses spatial operators to formulate the SKO dynamics and developefficient recursive algorithms for systems with non-rigid links. Spacecraft and otherlightweight structures are important examples of flexible multibody systems. Refer-ences [22,26,35,64,65,118,169] describe approaches to modeling the dynamics offlexible multibody systems with Banerjee [21] providing a historical survey of thefield. For the sake of simpler exposition, we will focus on a canonical serial-chainsystem, though generalizations to more general topologies systems along the linesdiscussed in Chap. 9 are straightforward.

13.1 Lumped Mass Model for a Single Flexible Body

In this section, we begin the development of the equations of motion for a canoni-cal serial-chain flexible multibody system with n flexible bodies. The formulationassumes that the body deformations are small, but allows large articulation at thehinges.

Each flexible body is assumed to have a lumped mass model consisting of acollection of nodal rigid bodies. Such models are typically developed using standardfinite element structural analysis techniques. Adopting the notation from Sect. 3.6on page 53, the number of nodes for the kth body is denoted nnd(k), and the jthnode on the kth body is referred to as the O

jk node. Each body has associated with

it a body reference frame denoted Bk for the kth body. The deformations of thenodes on the body are described with respect to the Bk body reference frame, whilethe rigid body motion of the kth body is characterized by the motion of the Bk

frame itself.


246 13 Systems with Link Flexibility

13.1.1 Equations of Motion of the Ojk Node

Each Ojk node frame undergoes translation and deformation with respect to the Bk

body frame. Assuming small deformation, the small linear and rotational defor-mations can be expressed as a spatial vector. The 6-dimensional spatial deforma-tion (slope plus translational) of node O

jk (with respect to frame Bk) is denoted

und(Ojk) ∈R6, and has the form:

und(Ojk) =

[

δq(Ojk)

δl(Ojk)

]

(13.1)

δq(Ojk) and δl(O

jk) denote the rotational and linear 3-vector deformation com-

ponents, respectively. The translational vector from Bk to the Ojk node reference

frame is denoted l(k,Ojk) ∈ R3, as illustrated in Fig. 13.1. l(k,Ojk), is deformationdependent and is given by:

l(k,Ojk) = l0(k,Ojk)+δl(Ojk) (13.2)

where l0(k,Ojk) denotes the undeformed vector. The spatial velocity of the Ojk

O+k

O+k−1

Ok

Ok−1

kth

hinge

(k−1)th

hinge

kth link

(k+1)th link(k−1)th link

Bk


l(Bk,Ojk)

und(Ojk)

Ojk node after

deformation

Ojk node before

deformation

Fig. 13.1 Illustration of links and hinges in a flexible serial multibody system

node with respect to the inertial frame is denoted V(

Ojk

)

, and is related to the

spatial velocity V(k) of the Bk body frame as follows:

V(

Ojk

)

= φ∗(k,Ojk)V(k)+ und(O

jk) ∈R6 (13.3)

13.1 Lumped Mass Model for a Single Flexible Body 247

und(Ojk) denotes the spatial velocity of the O

jk with respect to the Bk body frame.

Since φ∗(k,Ojk) depends on l(k,Ojk), its value is deformation dependent. The spa-

tial acceleration of the Ojk node, αnd(O

jk), is defined as

αnd(Ojk)

�=

dV(

Ojk

)

dt∈R6 (13.4)

Following the discussion in Sect. 5.1.3 on page 83, we have refrained from specify-ing the derivative frame in (13.4), since the choice is non-unique. The key require-ment however, is that, Coriolis and gyroscopic terms, that are consistent with thederivative frame choice, be used.

For the moment, we ignore forces from interactions with other bodies in thesystem. With this assumption, the equations of motion of each nodal body is thatof a standard rigid body – except for additional structural elastic strain forces onthe node. With Mnd(O

jk) ∈ R6×6 denoting the spatial inertia of the O

jk node, the

equations of motion of the Ojk node are those for a rigid body described in Chap. 2:

fnd(Ojk) =Mnd(O

jk)αnd(O

jk)+b(O

jk)+ f

stnd(O

jk) (13.5)

In (13.5), fstnd(Ojk) ∈R6 is the spatial elastic strain force at O

jk, while b(O

jk) ∈R6

denotes the (rigid body) gyroscopic spatial force for node Ojk. The expression for

b(Ojk) is depends on the specific derivative frame choice for αnd(O

jk) in (13.4).

13.1.2 Nodal Equations of Motion for the kth Flexible Body

Defining the overall deformation field, und(k), and nodal spatial velocity, Vnd(k),stacked vectors for the kth body as

und(k)�= col

{und(O

jk)

}nnd(k)

j=1∈R6nnd(k)

Vnd(k)�= col

{V(

Ojk

)}nnd(k)

j=1∈R6nnd(k) (13.6)

it follows from (13.3) that

Vnd(k) = B∗(k)V(k)+ und(k) (13.7)

where

B(k)�=

[

φ(

k,O1k

)

,φ(

k,O2k

)

, · · · ,φ(

k,Onnd(k)k

)]

∈R6×6nnd(k) (13.8)


Gathering together the equations of motion for the individual Ojk nodal bodies

on the kth body, we can express the equations of motion for the kth flexible bodyin the form:

fnd(k) =Mnd(k)αnd(k)+bnd(k)+Kst(k)und(k) (13.9)

where

αnd(k)�= col

{αnd(O

jk)

}nnd(k)

j=1∈R6nnd(k)

fnd(k)�= col

{fnd(O

jk)

}nnd(k)

j=1∈R6nnd(k)

bnd(k)�= col

{b(O

jk)

}nnd(k)

j=1∈R6nnd(k) (13.10)

TheMnd(k) structural mass matrix for the kth body is the block diagonal matrixdefined as

Mnd(k)�= diag

{Mnd(O

jk)

}nnd(k)

j=1∈R6nnd(k)×6nnd(k) (13.11)

while Kst(k)∈R6nnd(k)×6nnd(k) denotes the block-diagonal structural stiffnessmatrix for all the nodes on the kth flexible body. Both Mnd(k) and Kst(k) aretypically generated using finite element structural analysis.

The equations of motion above have focused on independent bodies, and haveignored interactions with the adjoining bodies. We remedy this in the followingsection.

13.1.3 Recursive Relationships Across the Flexible Bodies

The spatial velocity Vnd(O+k

) ∈R6 of node O+k

(on the inboard of the kth hinge)is related to the spatial velocity, V(k+ 1), of the (k+ 1)th body reference frameBk+1, as follows:

Vnd(O+k ) = φ∗

(k+ 1,O+k )V(k+ 1)+ und(O

+k ) (13.12)

Since the ΔV(k) relative spatial velocity across the kth hinge is H∗(k)θ(k), theV(Ok) spatial velocity of frame Ok on the outboard side of the kth hinge is

Vnd(Ok) = Vnd(O+k )+H∗

(k)θ(k) (13.13)

The V(k) spatial velocity of the kth body reference frame is given by

V(k) = φ∗(Ok,k)

[

Vnd(Ok)− und(Ok)]

(13.14)

13.2 Modal Formulation for Flexible Bodies 249

Putting together (13.12), (13.13) and (13.14), it follows that

V(k) = φ∗(k+ 1,k)V(k+ 1)+φ∗

(O+k ,k)und(O

+k )

+φ∗(Ok,k)

[

H∗(k)θ(k)− und(Ok)

]

(13.15)

Equation (13.15) provides a recursive relationship that relates the spatial velocity ofthe kth body frame to the spatial velocity of the (k+ 1)th body frame.

The equations of motion of the nodal bodies on the kth flexible body are as givenby (13.5), except for the O

+k−1 node, which is subject to inter-body interaction force

across the (k− 1)th hinge. The equations of motion of this particular node are asfollows:

fnd(O+k−1) = φ(O

+k−1,k− 1)f

rfl(k− 1)+Mnd(O

+k−1)αnd(O

+k−1)

+b(O+k−1)+ f

stnd(O

+k−1) (13.16)

wherefrfl(k)

�= B(k)fnd(k) (13.17)

frfl(k) ∈ R6 denotes the effective spatial force of interaction, referenced to frameBk, between the (k+1)th and kth bodies across the kth hinge. It thus follows thatthe nodal equations of motion for the kth flexible body in (13.9) need to be alteredto include this interaction force across the (k−1)th hinge. The modified form is asfollows:

fnd(k) = Cfl(k,k− 1)frfl(k− 1)+Mnd(k)αnd(k)

+bnd(k)+Kst(k)und(k)(13.18)

where

Cfl(k,k− 1)�=

⎡

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎣

0...

φ(O+k−1,k− 1)

...

0

⎤

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎦

∈R6nnd(k)×6 (13.19)

13.2 Modal Formulation for Flexible Bodies

Assumed modes provide a series expansion representation of the deformation fieldand are often used to represent the deformation of flexible bodies. A subset of themodes can be used for reduced order models for control system design. Lumpedmass models described in the previous sections are the starting point for developingassumed mode models.


There is a close relationship between the choice of a body reference frame andthe type of assumed modes. The complete motion of the flexible body is definedby the motion of the body reference frame and the deformation of the body withrespect to the body frame. In the multibody context, it is often convenient to choosethe location of the kth body reference frame Bk as a material point on the bodyfixed to node Ok at the inboard hinge. For this choice, the assumed modes arecantilever modes, and node Ok exhibits zero deformation (und(Ok) = 0). Free–freemodes are also used for representing body deformation, and are often preferred forcontrol analysis and design. For these modes, the reference frame Bk is not fixedto any node, but is rather assumed to be fixed to the undeformed body, and, as aresult, all nodes exhibit non-zero deformation. The dynamics models and algorithmsdeveloped here handle both types of modes. See Likins [118] for a discussion on thechoice of reference frame and modal representations for a flexible body.

Assume that a set of nmd(k) assumed modes has been chosen for the kth body.Let Πr(O

jk) ∈ R6 denote the modal spatial displacement vector at the O

jk

nodefor the rth mode, and η(k) ∈ Rnmd(k) denote the vector of modal deformationcoordinates for the kth body. TheΠr(O

jk)∈R6 modal spatial displacement vectors

are assumed to be constant and independent of the deformation of the body. Thespatial deformation of node O

jk is represented using modal coordinates as follows:

und(Ojk) =

nmd(k)∑r=1

Πr(Ojk)ηr(k) (13.20)

ηr(k) denotes the rth element of η(k). Equation (13.20) defines a linear relation-ship between the modal coordinates and the deformation spatial displacements atthe nodes. For cantilever modes,

Πr(Ok) = 0 for r= 1 · · ·nmd(k) (13.21)


und(Ojk) = Π(O

jk)η(k) (13.22)

where the modal spatial displacement influence vector Π(Ojk) for the O

jk node is

defined as:

Π(Ojk)

�=

[

Π1(Ojk), · · · ,Πnmd(k)(O

jk)

]nmd(k)

r=1∈R6×nmd(k)

The Π(k) modal matrix for the kth body is defined as follows:

Π(k)�= col

{Π(O

jk)

}nnd(k)

j=1∈R6nnd(k)×nmd(k)


and relates the modal coordinates to the deformation field for the kth body asfollows:

und(k)13.22= Π(k)η(k) (13.23)

The rth column of Π(k) is denoted Πr(k) ∈R6nnd(k), and is the mode shape forthe rth assumed mode for the kth body.

The modal generalized coordinates parametrize the deformation of a body, whilethe hinge generalized coordinates parametrize the large angle articulation motion ofthe body. Taken together, they completely characterize the motion of a flexible body.Therefore, define the ϑ(k) generalized coordinates and ϑ(k) generalized velocitiesvectors for the kth body as

ϑ(k)�=

[

η(k)

θ(k)

]

∈RN(k) and ϑ(k)�=

[

η(k)

θ(k)

]

∈RN(k) (13.24)

where N(k)�= nmd(k)+ rv(k) represents the overall number of velocity degrees

of freedom associated with the kth body.

13.2.1 Modal Mass Matrix for a Single Body

The kinetic energy of the kth body is the sum of the kinetic energies of the nodalelements and is given by

Ke(k) =12

nnd(k)∑j=1

V∗(

Ojj

)

Mnd(Ojk)V

(

Ojj

)

13.6,13.11=

12V∗nd(k)Mnd(k)Vnd(k)

=12V∗fl(k)

[

Π∗(k)B(k)

]

Mnd(k)[

Π(k), B∗(k)]

Vfl(k)

where

Vnd(k)13.7,13.23

=[

Π(k), B∗(k)]

Vfl(k) with Vfl(k)�=

[

η(k)

V(k)

]

∈RN(k) (13.25)

and N(k)�= nmd(k)+6. We refer to Vfl(k) as the modal spatial velocity for the

kth body. Thus, the kinetic energy of the kth body can be expressed as

Ke(k) =12V∗fl(k)Mfl(k)Vfl(k) (13.26)


where

Mfl(k)�=

⎡

⎣

Π∗(k)

B(k)

⎤

⎦Mnd(k)[Π(k), B∗(k)]

=

⎛

⎝

Π∗(k)Mnd(k)Π(k) Π∗(k)Mnd(k)B∗(k)

B(k)Mnd(k)Π(k) B(k)Mnd(k)B∗(k)

⎞

⎠

=

⎛

⎝

Mfffl(k) Mfr

fl(k)

Mrffl(k) Mrr

fl (k)

⎞

⎠ ∈RN(k)×N(k)

(13.27)

Corresponding to the generalized velocities vector ϑ(k), Mfl(k) as definedabove is the modal mass matrix of the kth body. In the block partitioning in(13.27), the superscripts f and r denote the flexible and rigid blocks, respectively,corresponding to the flex/rigid partitioning of the ϑ flexible generalized coordi-nates vector in (13.24). Thus,Mff

fl(k) represents the flex/flex coupling block, whileMfrfl(k) represents the flex/rigid coupling block of Mfl(k). We will use this nota-

tional convention throughout this chapter.Mrrfl (k) is precisely the rigid body spatial

inertia of the kth body. Indeed, Mfl(k) reduces to the rigid body spatial inertiawhen the body is rigid, since in this case nmd(k) = 0 (and Π(k) is null).

Since the vector l(k,Ojk) from Bk to node Ojk depends on the deformation of

the node, the operator B(k) is also deformation dependent. From (13.27) it followsthat, while the block Mff

fl(k) is deformation independent, both the blocks Mfrfl(k)

andMrrfl(k) are deformation dependent. The detailed expression for the modal mass

matrix can be defined using modal integrals which are computed as a part of thefinite-element structural analysis of the flexible bodies [169]. These expressions forthe modal mass matrix of the kth body were initially derived in [79]. Often thedeformation dependent parts of the modal mass matrix are ignored, and free–freeeigen-modes are used for the Π(k) assumed modes. When this is the case, Mfr

fl(k)

is zero andMfffl(k) is diagonal.

13.2.2 Recursive Relationships Using Modal Coordinates

Using modal coordinates, (13.15) can be re-expressed as:

V(k) = φ∗(k+ 1,k)V(k+ 1)+φ∗

(O+k ,k)Π(O

+k+1)η(k+ 1)

+φ∗(Ok,k)

[

H∗(k)θ(k)−Π(Ok)η(k)

]

(13.28)


Thus, the mVfl(k) nodal spatial velocity for the kth body can be expressed as

Vfl(k)13.28,13.25

= Φ∗fl(k+ 1,k)Vfl(k+ 1)+H∗

fl(k)ϑ(k) (13.29)

where the inter-body transformation matrixΦfl(k+ 1,k) ∈RN(k+1)×N(k) andthe modal joint map matrix Hfl(k) ∈RN(k)×N(k) are defined as

Φfl(k+ 1,k)�=

(

0 Π∗(O+k+1)φ(O

+k ,k)

0 φ(k+ 1,k)

)

(13.30)

Hfl(k)�=

(

I −Π∗B(Ok)

0 HB(k)

)

(13.31)

with

HB(k)�= H(k)φ(Ok,k) ∈Rrv(k)×6

and ΠB(Ok)�= φ∗

(Ok,k)Π(Ok) ∈R6×N(k)

13.2.3 Recursive Propagation of Accelerations

Differentiating the velocity recursion equation (13.29), we obtain the following re-cursive expression for the modal spatial acceleration αfl(k) ∈RN(k) for the kthbody:

αfl(k)�=

dVfl(k)

dt=

[

η(k)

α(k)

]

= Φ∗fl(k+ 1,k)αfl(k+ 1)+H∗

fl(k)ϑ(k)+afl(k) (13.32)

whereα(k)�= V(k), and the Coriolis acceleration term afl(k)∈RN(k) is given by

afl(k) =dΦ∗fl(k+ 1,k)

dtVfl(k+ 1)+

dH∗fl(k)

dtϑ(k) (13.33)

The relationship of the αnd(k) vector of spatial accelerations of all the nodes forthe kth body to the αfl(k) vector is obtained by differentiating (13.7):

αnd(k) =dVnd(k)

dt=[

Π(k), B∗(k)]

αfl(k)+and(k) (13.34)

where

and(k)�= col

{a(O

jk)

}=

d[Π(k), B∗(k)]dt

Vfl(k) ∈R6nnd(k) (13.35)


The time derivative frames used in (13.32) and (13.35) are assumed to be consistentwith the derivative frame choice in (13.4).

13.2.4 Recursive Propagation of Forces

Using the principle of virtual work, it follows from (13.25) that the modal spatialforces, ffl(k) ∈RN(k), for the kth body is given by

ffl(k)�=

[

Π∗(k)B(k)

]

fnd(k)13.17=

[

Π∗(k)fnd(k)

ffl(k)

]

(13.36)

Pre-multiplying (13.18) by

[

Π∗(k)B(k)

]

, and using (13.27), (13.34), and (13.36), leads

to the following recursive relationship for the modal spatial forces:

ffl(k) =

[

Π∗(k)Cfl(k,k− 1)

B(k)Cfl(k,k− 1)

]

frfl(k− 1)+Mfl(k)αfl(k)

+bfl(k)+K(k)ϑ(k)

=

[

Π∗(O+k )

φ(k,O+k−1)

]

φ(O+k−1,k− 1)f

rfl(k− 1)+Mfl(k)αfl(k)

+bfl(k)+K(k)ϑ(k)

= Φfl(k,k− 1)ffl(k− 1)+Mfl(k)αfl(k)+bfl(k)+K(k)ϑ(k) (13.37)

Here, we have defined

bfl(k)�=

[

Π∗(k)B(k)

]

[bnd(k)+Mnd(k)and(k)] ∈RN(k) (13.38)

and the modal stiffness matrix

K(k)�=

(

Π∗(k)Kst(k)Π(k) 00 0

)

∈RN(k)×N(k) (13.39)

Equation (13.37) establishes a recursive relationship between the modal spatialforces across adjoining bodies. Since the columns of B∗(k) are indeed the defor-mation dependent rigid body modes for the kth body, they do not contribute to itselastic strain energy and lead to the expression for K(k) in (13.39). Indeed, when adeformation dependent structural stiffness matrix Kst(k) is used

Kst(k)B∗(k) = 0 (13.40)

13.3 SKO Models for Flexible Body Systems 255

However, the common practice (also followed here) of using a constant,deformation-independent structural stiffness matrix leads to a situation wherein(13.40) does not hold exactly. We neglect these non-zero terms on the left-handside of (13.40). The velocity-dependent bias term bfl(k) is formed using modalintegrals generated using structural analysis programs.

13.2.5 Overall Equations of Motion

Gathering together the recursive equations in (13.29), (13.32) and (13.37), we obtainthe following overall equations of motion for the system:

⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

Vfl(n+ 1) = 0, αfl(n+ 1) = 0

for k = n · · ·1Vfl(k) =Φ∗

fl(k+ 1,k)Vfl(k+ 1)+H∗fl(k)ϑ(k)

αfl(k) =Φ∗fl(k+ 1,k)αfl(k+ 1)+H∗

fl(k)ϑ(k)+afl(k)

end loop

(13.41)⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

ffl(0) = 0

for k = 1 · · ·nffl(k) =Φfl(k,k− 1)ffl(k− 1)+Mfl(k)αfl(k)+bfl(k)+K(k)ϑ(k)

Tfl(k) =Hfl(k)ffl(k)

end loop

As discussed in Remark 5.2.1 on page 85, external forces on the kth body can behandled by absorbing them into the bfl(k) gyroscopic force term.

13.3 SKO Models for Flexible Body Systems

The overall vectors of generalized configuration coordinates, ϑ, and generalized ve-locities, ϑ, for the serial multibody system are defined as

ϑ�= col

{ϑ(k)

}nk=1

∈RNfl and ϑ�= col

{ϑ(k)

}nk=1

∈RNfl (13.42)

where Nfl�=

∑nk=1 N(k) denotes the overall number of degrees of freedom for

the multibody system. The state of the multibody system is defined by the pair ofvectors {ϑ, ϑ}. For a given system state {ϑ, ϑ}, the equations of motion defining therelationship between the vector of generalized accelerations ϑ and the vector ofgeneralized forces Tfl ∈RNfl are developed in this section.


With N =∑nk=1 N(k), we use recursive velocity relationship in (13.29) to define

the EΦfl andΦfl SKO and SPO operators, respectively, for the system as:

EΦfl�=

∑k

e℘(k)Φfl(℘(k),k)e∗k and Φfl

�= (I−EΦfl)

−1 (13.43)

The (k,k−1) weight matrix for the EΦfl SKO operator isΦfl(k,k−1) based on thevelocity relationships in (12.3), the weights satisfy the usual semi-group property:

Φfl(k, j)�= Φfl(k,k− 1) · · ·Φfl(j+ 1, j) for k > j

It is also noteworthy that the weight matrices are neither invertible nor 6×6 matri-ces, as is the case for systems with rigid links. Moreover, the weight matrices are ofnon-uniform size for systems with a mix of rigid and flexible-links, and when thenumber of modes varies from body to body.

With our assumption of a canonical serial-chain system, EΦfl and Φfl have theform:

EΦfl =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0Φfl(2,1) 0 . . . 0 0

0 Φfl(3,2) . . . 0 0...

.... . .

......

0 0 . . . Φfl(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

Φfl =

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0Φfl(2,1) I . . . 0

......

. . ....

Φfl(n,1) Φfl(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

(13.44)

Also, define the spatial operatorHfl�= diag

{Hfl(k)

}nk=1

∈RNfl×N. Using these

spatial operators, and defining Vfl�= col

{Vfl(k)

}nk=1

∈RN from (13.29), it fol-

lows that the spatial operator expression for Vfl is given by

Vfl =Φ∗flH

∗fl ϑ (13.45)

Defining afl = col{

afl(k)}nk=1

∈ RN and αfl = col{αfl(k)

}nk=1

∈ RN, and

using spatial operators, we can re-express (13.32) in the form

αfl =Φ∗fl(H

∗fl ϑ+afl) (13.46)

13.3 SKO Models for Flexible Body Systems 257

From (13.37), the operator expression for the modal spatial forces ffl�=

col{ffl(k)

}nk=1

∈RN for all the bodies in the chain is given by

ffl =Φfl(Mflαfl+bfl+Kϑ) (13.47)

where

Mfl�= diag

{Mfl(k)

}nk=1

∈RN×N

K�= diag

{K(k)

}nk=1

∈RN×N

and bfl�= col

{bfl(k)

}nk=1

∈RN

From the principle of virtual work, the generalized forces vector Tfl ∈RNfl for themultibody system is given by the expression

Tfl =Hflffl (13.48)

13.3.1 Operator Expression for the System Mass Matrix

Collecting together the operator expressions in (13.45)–(13.48), we have:

Vfl =Φ∗flH

∗fl ϑ

αfl =Φ∗fl(H

∗fl ϑ+afl)

ffl =Φfl(Mflαfl+bfl+Kϑ)

Tfl =Hflffl = Mfl ϑ+Cfl

(13.49)

whereMfl

�= HflΦflMflΦ

∗flH

∗fl

and Cfl�= HflΦfl(MflΦ

∗flafl+bfl+Kϑ)

(13.50)

Here, Mfl ∈ RNfl×Nfl is the system mass matrix for the serial-chain, and the ex-pression HflΦflMflΦ

∗flH

∗fl is the Newton–Euler Operator Factorization of the

mass matrix. Cfl ∈ RNfl is the vector of Coriolis, gyroscopic, and elastic forcesfor the system. The above define an SKO model for the system.

13.3.2 Illustration of the SKO Formulation Procedure

Let us recap the development so far to highlight its parallels with the SKO formula-tion steps described in Sect. 9.6 on page 182.


1. In addition to the 6-dimensional V(k) spatial velocity of its body frame, thevelocity state of a body is defined by its nmd(k) dimensional η(k) deformationrate coordinates. Thus, the N(k) = nmd(k)+ 6 dimensional free-body general-ized velocities vector Vfl(k) for the kth body is defined as:

Vfl(k)13.25=

[

η(k)

V(k)

]

∈RN(k)

2. The appropriate inertia term for the kth flexible body is defined by the followingpartitioned matrix:

Mfl(k)13.27=

(

Mfffl(k) Mfr

fl(k)

Mrffl(k) Mrr

fl(k)

)

∈RN(k)×N(k)

The kinetic energy of the kth body, including both the rigid and deformationcontributions, is given by:

Ke(k) =12V∗fl(k)Mfl(k)Vfl(k)

3. The velocity recursion relating theMfl(k) velocity of the kth body to that of itsparent is expressed as

Vfl(k)13.25= Φ∗

fl(k+ 1,k)Vfl(k+ 1)+H∗fl(k)ϑ(k)

The inter-body transformation operator Φfl(k+ 1,k), the modal joint map ma-trix Hfl(k), and the independent ϑ(k) generalized coordinates, for the kth bodyare defined by (13.30), (13.31) and (13.24), respectively. The nmd(k)× 6 di-mensional Π∗(O+

k+1) and Π∗B(Ok) modal influence matrices are obtained from

the structural analysis of the body and define the mapping to the deformation ofthe nodes on either side of the kth hinge. From this, the SKO weight matricesare identified to be theΦfl(k+ 1,k) terms.

4. The Vfl and ϑ stacked vectors are assembled as usual. The EΦfl SKO operatoris defined with the Φfl(k+ 1,k) weight matrices. The SPO operator is definedby Φfl = (I−EΦfl)

−1 in (13.44). The block-diagonal Hfl and Mfl operatorsare defined using the Hfl(k) and Mfl(k) component matrices. These lead tothe operator-level equations of motion in (13.49) and the definition of the SKOformulation mass matrix and its Newton–Euler Factorization in (13.50):

Mfl�= HflΦflMflΦ

∗flH

∗fl

This defines an SKO model for the system.

13.4 Inverse Dynamics Algorithm 259

5. The analytical techniques and algorithms for the SKO formulation developed inSect. 9.2 are now directly applicable to the current formulation.

6. While it is premature to discuss specific algorithmic optimization issues, it isnevertheless worthwhile highlighting the sparsity of Φfl(k+ 1,k) and Hfl(k)in (13.30). A closer look at (13.29) reveals that the computational evaluation ofVfl(k) requires only the evaluation of the lower part of V(k), since the upperpart is a simple pass through. Thus, an obvious optimization of (13.29) is to usethe partitioned sub-blocks of the Φfl(k+ 1,k) and Hfl(k) matrices, instead ofthe full matrices, for improving algorithmic efficiency. Such optimization can bereadily applied to other algorithms, e.g., for computing the mass matrix, solvingthe forward dynamics problem, as will be seen in later sections.

13.4 Inverse Dynamics Algorithm

The inverse dynamics problem consists of computing the vector of generalizedforces Tfl for a prescribed set of generalized accelerations ϑ. The forward dynamicsproblem is the converse one, and requires the computation of the set of generalizedaccelerations ϑ resulting from a set of generalized forces Tfl.

The equations of motion in (13.41) are written in the form of a recursive Newton–Euler inverse dynamics algorithm. As expected, the structure of this algorithmclosely resembles the recursive Newton–Euler inverse dynamics algorithm for rigidmultibody systems seen in Chap. 9. This section describes simplifications of the in-verse dynamics algorithm obtained by exploiting the structure of the componentmatrices and vectors. Observe that

Φfl(k+ 1,k) = Afl(k+ 1)Bfl(k+ 1,k) (13.51)

where

Afl(k)�=

[

Π∗(O+k )

φ(k, O+k−1)

]

∈RN(k)×6

Bfl(k+ 1,k)�= [0, φ(O

+k ,k)] ∈R6×N(k)

(13.52)

Also, the modal joint map matrix Hfl(k) can be partitioned as

Hfl(k) =

[

HMfl(k)

HRfl(k)

]

∈RN(k)×N(k) (13.53)

where

HMfl(k)�= [I, −Π∗

B(Ok)] ∈Rnmd(k)×N(k)

and HRfl(k)�= [0, HB(k)] ∈Rrv(k)×N(k) (13.54)


Using block partitioning and the superscripts f and r that denote the flexible andrigid components, we have

Vfl(k) =

[

Vffl(k)

Vrfl(k)

]

αfl(k) =

[

αffl(k)

αrfl(k)

]

ffl(k) =

[

fffl(k)

frfl(k)

]

, and Tfl(k) =

[

Tffl(k)

Trfl(k)

]

By taking advantage of the special structure ofΦfl(k+ 1,k) andHfl(k) in (13.52)and (13.53), the Newton–Euler inverse dynamics recursions in (13.41) can be sim-plified to obtain Algorithm 13.1 for flexible-link system inverse dynamics.

Algorithm 13.1 Inverse dynamics for flexible-link systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

Vfl(n+ 1) = 0, αfl(n+ 1) = 0

for k = n · · ·1Vffl(k) = η(k)

Vrfl(k) = φ∗(O

+k ,k)A∗

fl(k+ 1)Vfl(k+ 1)

+H∗B(k)θ(k)−ΠB(Ok)η(k)

αffl(k) = η(k)

αrfl(k) = φ∗(O

+k ,k)A∗

fl(k+ 1)αfl(k+ 1)

+H∗B(k)θ(k)−ΠB(Ok)η(k)+a

rfl(k)

end loop

(13.55)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

ffl(0) = 0

for k = 1 · · ·nffl(k) = Afl(k)φ(O

+k−1,k− 1)f

rfl(k− 1)+Mfl(k)αfl(k)

+bfl(k)+K(k)ϑ(k)

Tfl(k) =

[

Tffl(k)

Trfl(k)

]

=

[

fffl(k)−Π∗B(Ok)f

rfl(k)

HB(k)frfl(k)

]

end loop

Flexible multibody systems typically have actuators only at the hinges. Thus,for the kth body, only the subset of the Tfl(k) generalized forces vector cor-responding to the Trfl(k) hinge actuator forces can be set, while the remain-ing Tffl(k) generalized forces are zero. Thus, in contrast with rigid multibody

13.5 Mass Matrix Computation 261

systems, flexible multibody systems are under-actuated systems, since the numberof available actuators is less than the number of motion degrees of freedom in thesystem. For such under-actuated systems, the inverse dynamics computations forthe Tfl generalized force are meaningful only when the input ϑ generalized accel-erations form a consistent data set.

13.5 Mass Matrix Computation

Now we describe a composite-body-based recursive algorithm for the computationof the mass matrix Mfl. This algorithm is based upon the following lemma whichcontains a decomposition of the mass matrix into block diagonal, block upper-triangular and block lower-triangular components.

Lemma 13.1 Decomposition of ΦflMflΦ∗fl for flexible-link systems.

Define the composite body inertias Rfl(k) ∈RN(k)×N(k) recursively for all thebodies in the serial-chain as follows:

⎧⎪⎪⎪⎨⎪⎪⎪⎩

Rfl(0) = 0

for k = 1 · · ·nRfl(k) =Φfl(k,k− 1)Rfl(k− 1)Φ∗

fl(k,k− 1)+Mfl(k)

end loop

(13.56)

Also, define Rfl�= diag

{Rfl(k)

}∈ RN×N. Then we have the following spatial

operator decomposition

ΦflMflΦ∗fl = Rfl+ ΦflRfl+RflΦ

∗fl (13.57)

where Φfl�= Φfl− I:

Proof: See Lemma 9.3 on page 166.Physically, Rfl(k) is the modal mass matrix of the composite body formed from

all the bodies outboard of the kth hinge by freezing all their (deformation plushinge) degrees of freedom. It follows from (13.50) and Lemma 13.1 that

Mfl = HflΦflMflΦ∗flH

∗fl

= HflRH∗fl+HflΦflRH

∗fl+HflRΦ

∗flH

∗fl (13.58)

The three terms on the right of (13.58) are block diagonal, block lower-triangularand block upper-triangular, respectively. Algorithm 13.2 for computing the massmatrix Mfl computes the elements of these terms recursively.


Algorithm 13.2 The mass matrix for flexible-link systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

R(0) = 0

for k = 1 · · ·nR(k) =Φfl(k,k− 1)R(k− 1)Φfl

∗(k,k− 1)+Mfl(k)

= Afl(k)φ(O+k−1,k− 1)Rrr(k− 1)φ∗

(O+k−1,k− 1)A∗

fl(k)

+Mfl(k)

X(k) = R(k)H∗fl(k)

Mfl(k,k) =Hfl(k)X(k)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

for j = (k+ 1) · · ·nX(j) =Φfl(j, j− 1)X(j− 1)

= Afl(j)φ(O+j−1, j− 1)Xr(j− 1)

Mfl(j,k) = Mfl∗(k, j) =Hfl(j)X(j)

end loop

end loop

The main recursion proceeds from tip to base, and computes the blocks along thediagonal of Mfl. As each such diagonal element is computed, a recursion to com-pute the off-diagonal elements is spawned. The structure of this algorithm closelyparallels the composite rigid body Algorithm 9.3 for computing the mass matrix forSKO models. Some computational simplifications of the algorithm arising from thesparsity of HMfl(k) andHRfl(k) have been included in the steps in the algorithm.

13.6 Factorization and Inversion of the Mass Matrix

We now follow the process defined in Sects. 9.4 and 9.5 for an SKO model to de-velop the Innovations Operator Factorization of the mass matrix. Algorithm 13.3describes the recursive algorithm for the required articulated body quantities. Theoperator Pfl ∈ RN×N is defined as a block diagonal matrix with the kth diago-nal element being Pfl(k). The quantities defined in (13.59) form the componentelements of the following spatial operators:

13.6 Factorization and Inversion of the Mass Matrix 263

Algorithm 13.3 Standard AB algorithm for flexible-link systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+fl(0) = 0

for k = 1 · · ·nPfl(k) =Φfl(k,k− 1)P+

fl(k− 1)Φ∗fl(k,k− 1)+Mfl(k)

Dfl(k) =Hfl(k)Pfl(k)H∗fl(k)

Gfl(k) = Pfl(k)H∗fl(k)D

−1fl (k)

Kfl(k+ 1,k) =Φfl(k+ 1,k)Gfl(k)

τ(k) = I−Gfl(k)Hfl(k)

P+fl(k) = τ(k)Pfl(k)

Ψfl(k+ 1,k) =Φfl(k+ 1,k)τ(k)

end loop

(13.59)

Dfl�= HflPflH

∗fl = diag

{Dfl(k)

}nk=1

∈RNfl×Nfl

Gfl�= PflH

∗flDfl

−1= diag

{Gfl(k)

}nk=1

∈RN×Nfl

Kfl�= EΦflGfl ∈RN×Nfl

τ�= I−GflHfl = diag

{τ(k)

}nk=1

∈RN×N

EΨfl�= EΦflτ ∈RN×N

(13.60)

The only non-zero block elements of Kfl and EΨfl are Kfl(k+ 1,k) andΨfl(k+ 1,k) elements, respectively, along the first sub-diagonal.

As in the case for EΦfl , EΨfl is an SKO operator and thus its corresponding SPOoperator, Ψfl, is as follows:

Ψfl�= (I−EΨfl)

−1=

⎛

⎜

⎜

⎜

⎜

⎝

I 0 . . . 0

Ψfl(2,1) I . . . 0...

.... . .

...

Ψfl(n,1) Ψfl(n,2) . . . I

⎞

⎟

⎟

⎟

⎟

⎠

∈RN×N (13.61)

where

Ψfl(i, j)�= Ψfl(i, i− 1) · · · Ψfl(j+ 1, j) for i > j

The structure of the EΨfl and Ψfl operators is identical to that of the EΦfl andΦfloperators. The Innovations Operator Factorization of the mass matrix is defined inthe following lemma.


Lemma 13.2 Mass matrix factorization and inversion for flexible linksystems.

Mfl = [I+HflΦflKfl]Dfl[I+HflΦflKfl]∗

[I+HflΦflKfl]−1

= [I−HflΨflKfl]

M−1= [I−HflΨflKfl]

∗D−1fl [I−HflΨflKfl]

ϑ= [I−HflΨflKfl]∗D−1fl

[

Tfl−HflΨfl{KflTfl

+Pflafl+bfl+Kϑ}]

−K∗flΨ

∗flafl

(13.62)

Proof: See Lemmas 9.11 and 9.12.Once again, the factor [I−HflΨflKfl] is square, block lower-triangular and non-

singular, and so Lemma 13.2 provides an analytical closed-sform expression for theblock LDL∗ decomposition of M−1.

13.7 AB Forward Dynamics Algorithm

Equation (13.62) provides an expression for the ϑ generalized accelerations. Thisexpression directly leads to the recursive Algorithm 13.4 for the forward dynamicsof the system. The structure of this algorithm is identical in form to the AB articu-lated body Algorithm 9.5 on page 182.

In accordance with the optimization step in SKO formulation process in Sect. 9.6on page 182, we now look into exploiting the sparsity and special structure of thecomponent matrices in (13.52)–(13.54) to further optimize the forward dynamicsalgorithm.

13.7.1 Simplified Algorithm for the Articulated Body Quantities

Instead of a detailed derivation, we describe here the conceptual basis for a separa-tion of the modal and hinge degrees of freedom for each body, used in the optimiza-tion process. First, we recall the velocity recursion equation in (13.29)

Vfl(k) =Φ∗fl(k+ 1,k)Vfl(k+ 1)+H∗

fl(k)ϑ(k) (13.64)

and the partitioned form of Hfl(k) in (13.31)

Hfl(k) =

[

HMfl(k)

HRfl(k)

]

(13.65)

13.7 AB Forward Dynamics Algorithm 265

Algorithm 13.4 Standard AB forward dynamics algorithm for flexible-linksystems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

z+(0) = 0

for k = 1 · · ·nz(k) =Φfl(k,k− 1)z

+(k− 1)+Pfl(k)afl(k)+bfl(k)+K(k)ϑ(k)

ε(k) = Tfl(k)−Hfl(k)z(k)

ν(k) = D−1fl (k)ε(k)

z+

(k) = z(k)+Gfl(k)ε(k)

end loop

(13.63)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

αfl(n+ 1) = 0

for k = n · · ·1α+fl(k) =Φ∗

fl(k+ 1,k)αfl(k+ 1)

ϑ(k) = ν(k)−Gfl∗(k)α+

fl(k)

αfl(k) = α+fl(k)+H∗

fl(k)ϑ(k)+afl(k)

end loop

Introducing a dummy variable k ′, we can rewrite (13.64) as

Vfl(k′) = Φ∗

fl(k+ 1,k ′)Vfl(k+ 1)+H∗Mfl(k)η(k)

Vfl(k) = Φ∗fl(k

′,k)Vfl(k ′)+H∗Rfl(k)θ(k) (13.66)

whereΦfl(k+ 1,k ′) �

= Φfl(k+ 1,k) and Φfl(k′,k) �

= I6

Conceptually, each flexible body is replaced by two new bodies. The first body isassigned the kinematical and mass/inertia properties of the flexible body and alsoits deformation generalized coordinates. The second virtual body is mass-less, andhas zero extent. It is assigned the hinge generalized coordinates. The serial-chainnow contains twice the number of bodies as the original one, with half the new bod-ies being fictitious ones. The tree digraph associated with the new system containstwice as many nodes. The new H∗

fl operator now has the same number of columns,but twice the number of rows, as the original H∗

fl operator. The new Φfl operatorhas twice as many rows and columns as the original one. Repeating the analysis de-scribed in the previous sections, we once again obtain the SKO operator expressionas (13.62). This expression also leads to a recursive forward dynamics algorithmin Algorithm 13.5. However, each sweep in the algorithm now contains twice asmany steps as the original algorithm. However, since each step now processes only


Algorithm 13.5 Two stage AB algorithm for flexible-link systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+fl(0) = 0

for k = 1 · · ·nΓfl(k) = Bfl(k,k− 1)P+

fl(k− 1)B∗fl(k,k− 1) ∈R6×6

Pfl(k) = Afl(k)Γfl(k)A∗fl(k)+Mfl(k) ∈RN(k)×N(k)

Dm(k) =HMfl(k)Pfl(k)H∗Mfl(k) ∈Rnmd(k)×nmd(k)

Gm(k) = Pfl(k)H∗Mfl(k)D

−1m (k) ∈RN(k)×nmd(k)

τm(k) = I−Gm(k)HMfl(k) ∈RN(k)×N(k)

Pcr(k) = τm(k)Pfl(k) ∈RN(k)×N(k)

Dcr(k) =HRfl(k)Pcr(k)H∗Rfl(k) ∈Rrv(k)×rv(k)

Gcr(k) = Pcr(k)H∗Rfl(k)D

−1cr (k) ∈RN(k)×rv(k)

τcr(k) = I−Gcr(k)HRfl(k) ∈RN(k)×N(k)

P+fl(k) = τcr(k)Pcr(k) ∈RN(k)×N(k)

Ψfl(k+ 1,k) =Φfl(k+ 1,k)τ(k) ∈RN(k)×N(k)

end loop

(13.67)

a smaller number of degrees of freedom, this leads to a reduction in the overall cost.The new algorithm (replacing Algorithm 13.3) for computing the articulated bodyquantities is described in Algorithm 13.5.

We now use the sparsity of Bfl(k+ 1,k),HMfl(k) andHRfl(k) to further sim-plify the above algorithm. Using the symbol “×” to indicate “don’t care” blocks,the structure in block partitioned form of some of the quantities in Algorithm 13.5is shown in Fig. 13.2. Using the structure described above, the simplified algorithmfor computing the articulated body quantities is described in Algorithm 13.6.

13.7.2 Simplified AB Forward Dynamics Algorithm

Analogous to Algorithm 9.5, the complete recursive articulated body forward dy-namics Algorithm 13.7 for a serial flexible multibody system follows directly fromthe recursive implementation of the expression in (13.62). The algorithm consists ofthe following steps:

1. A base-to-tip recursion as in (13.55) for computing the modal spatial veloci-ties Vfl(k) and the Coriolis and gyroscopic terms afl(k) and bfl(k) for all thebodies.


Γfl(k) =φ(O+k−1,k−1)P+

pr(k−1)φ∗(O+k−1,k−1), (P+

pr(k) is defined below)

Gm(k) =

[

×gfl(k)

]

where gfl(k)�= μfl(k)D−1

m (k) ∈R6×nmd(k)

and μfl(k)�= [Prffl(k), Prrfl(k)]H∗

Mfl(k) ∈R6×nmd(k)

Pcr(k) =

(

× ×× Ppr(k)

)

, where Ppr(k)�= Prrfl(k)−gfl(k)μ∗

fl(k) ∈R6×6

Dcr(k) =HB(k)Ppr(k)H∗B(k) ∈Rrv(k)×rv(k)

Gcr(k) =

[

×Gpr(k)

]

, where Gpr(k)�= Ppr(k)H∗

B(k)D−1cr (k) ∈R6×rv(k)

τcr(k) =

(

I ×0 τpr(k)

)

where τpr(k)�= I−Gpr(k)HB(k) ∈R6×6

P+fl(k) =

(

× ×× P+

pr(k)

)

, where P+pr(k) = τpr(k)Ppr(k) ∈R6×6

(13.68)

Fig. 13.2 The partitioned structure of the articulated body inertia quantities for flexible-link systems

2. Computation of the articulated body quantities using Algorithm 13.6.3. A tip-to-base recursion followed by a base-to-tip recursion for the joint acceler-

ations ϑ as described in Algorithm 13.7.

The recursion in Algorithm 13.7 is obtained by simplifying the recursions in (13.63)in the same manner as described in the previous section for the articulated bodyquantities. The recursive forward dynamics algorithm described here is based onreferences [146, 147]. Other alternate recursive forward dynamics approaches forflexible-link systems are described in [99, 104].

It has been pointed out in the literature [98,133] that the use of modes for model-ing body flexibility leads to “premature linearization” of the dynamics, in the sensethat while the dynamics model contains deformation dependent terms, the geomet-ric stiffening terms are missing. These missing geometric stiffening terms are thedominant terms among the first order (deformation) dependent terms. In general, itis necessary to take additional steps to recover the missing geometric stiffness termsto obtain a “consistently” linearized model with the proper degree of fidelity. How-ever, for systems with low spin rate, there is typically little loss in model fidelitywhen the deformation and deformation rate dependent terms are dropped altogetherfrom the terms in the dynamical equations of motion [133]. These linearized modelsare considerably less complex, and do not require most of the modal integrals datafor each individual flexible body. One such approximation is to ignore the depen-


Algorithm 13.6 Simplified computation of the articulated body inertias forflexible-link systems⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+pr(0) = 0

for k = 1 · · ·nΓfl(k) = φ(O

+k−1,k− 1)P+

pr(k− 1)φ∗(O

+k−1,k− 1)

Pfl(k) = Afl(k)Γfl(k)A∗fl(k)+Mfl(k)

Dm(k) =HMfl(k)Pfl(k)H∗Mfl(k)

μfl(k) = [Prffl(k), Prrfl (k)]H∗Mfl(k)

gfl(k) = μfl(k)D−1m (k)

Ppr(k) = Prrfl (k)−gfl(k)μ∗fl(k)

Dpr(k) =HB(k)Ppr(k)H∗B(k)

Gpr(k) = Ppr(k)H∗B(k)D−1

pr (k)

τpr(k) = I−Gpr(k)HB(k)

P+pr(k) = τpr(k)Ppr(k)

end loop

(13.69)

dency ofMfl(k) on the body deformation and treat it as being constant in the bodyframe. Simplifications arising from this are discussed in the following exercise.

Exercise 13.1 Additional forward dynamics simplifications.With the assumption that Mfl(k) is independent of the body deformation, show

that the formation of D−1m in (13.69) can be simplified as follows:

D−1m (k) = Lfl(k)−Ufl(k)

[

Γ−1fl (k)+Ofl(k)

]−1U∗fl(k) (13.70)

The matrices Lfl(k), Ofl(k), and Ufl(k) are configuration independent and needto be computed just once as follows:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

for k = 1 · · ·nLfl(k) =

[

HMfl(k)Mfl(k)H∗Mfl(k)

]−1 ∈RNfl×Nfl

ζ(k) =HMfl(k)Afl(k) ∈RNfl×6

Ufl(k) = Lfl(k)ζ(k) ∈RNfl×6

Ofl(k) = ζ∗(k)Ufl(k) ∈R6×6

end loop

(13.71)


Algorithm 13.7 Simplified AB forward dynamics for flexible-link systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

z+pr(0) = 0

for k = 1 · · ·n

z(k) =

[

zm(k)

zcr(k)

]

= Afl(k)φ(O+k−1,k− 1)z

+pr(k− 1)

+bfl(k)+K(k)ϑ(k) ∈RN(k)

εm(k) = Tm(k)− zm(k)+Π∗B(Ok)zcr(k) ∈Rnmd(k)

νm(k) = D−1m (k)εm(k) ∈Rnmd(k)

zpr(k) = zcr(k)+gfl(k)εm(k)+Ppr(k)amR(k) ∈R6

εpr(k) = Tpr(k)−HB(k)zpr(k) ∈Rrv(k)

νpr(k) = D−1pr(k)εpr(k) ∈Rrv(k)

z+pr(k) = zpr(k)+Gpr(k)εpr(k) ∈R6

end loop⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

αfl(n+ 1) = 0

for k = n · · ·1α+pr(k) = φ∗

(O+k ,k)A∗

fl(k+ 1)αfl(k+ 1) ∈R6

θ(k) = νpr(k)−G∗pr(k)α

+pr(k) ∈Rrv(k)

αpr(k) = α+pr(k)+H∗

B(k)θ(k)+amR(k) ∈R6

η(k) = νm(k)−g∗fl(k)αpr(k) ∈Rnmd(k)

αfl(k) =

[

η(k)

αpr(k)−ΠB(Ok)η(k)

]

∈RN(k)

end loop

Using (13.70) reduces the computational cost for computing the articulated bodyinertias to a quadratic rather than a cubic function of the number of modes.

Part IIIAdvanced Topics

Chapter 14Transforming SKO Models

In this chapter we explore techniques for partitioning SKO models. These aresubsequently used to develop methods for aggregating sub-graphs to transform SKOmodels into new ones. In Chap. 15, sub-graph aggregation forms the basis of con-strained embedding techniques to develop SKO models for closed-chain multibodysystems.

14.1 Partitioning Digraphs

A sub-graph S of a digraph, T, is defined as a digraph containing a subset of thenodes and edges in T. S is said to be induced if all edges in T connecting nodepairs in S are also in S [38,183]. In other words, S is induced if all node pairs in Sthat are adjacent in T are also adjacent in S. Thus induced sub-graphs preserve theadjacency property for node pairs, so that a pair of nodes in an induced sub-graphare adjacent if and only if they are adjacent in the parent digraph. The induced sub-graph SI for a sub-graph S is the minimal sub-graph containing S that is also aninduced sub-graph. A sub-graph, and its induced sub-graph, contain the same nodes,and differ only in the edges they contain.

A sub-graph S of T is said to be path-induced if it contains all the paths (nodesand edges) in T that connect node pairs in S. A path-induced sub-graph has nomissing nodes or edges for paths in T that connect the nodes in S. Thus path-induced sub-graphs preserve the relatedness property for node pairs, so that a pairof nodes in a path-induced sub-graph are related if and only if they are relatedin the parent digraph. The path-induced sub-graph SP for a sub-graph S is theminimal sub-graph of T containing S that is path-induced. Figure 14.1 illustrates asub-graph, and its induced and path-induced sub-graphs. The path-induced propertyapplies even to disconnected sub-graphs. A path-induced sub-graph, will generallycontain more nodes and edges than the original sub-graph or its induced sub-graph.That is,

S ⊆ SI ⊆ SP (14.1)


274 14 Transforming SKO Models

sub-graph S path-inducedsub-graphSP

inducedsub-graphSI

Fig. 14.1 Examples of a sub-graph S and its induced and path-induced sub-graphs SI andSP , respectively

Exercise 14.1 Non-tree path-induced sub-graphs.Let S be a path-induced sub-graph of a digraph T. Show that:

1. If S contains an edge that is a part of a directed cycle in T, then the full cyclemust also be in S.

2. If S contains a pair of multiply-connected nodes (i.e., nodes connected by morethan one path), then all the paths connecting them must also be in S.

14.1.1 Partitioning by Path-Induced Sub-Graphs

We now turn our attention to partitions generated by path-induced sub-graphs.

Lemma 14.1 Induced partitions of digraphs.Assume that S is a path-induced sub-graph of a digraph T. Define the C and P

sub-graphs as follows:

• C child sub-graph is the induced sub-graph for the set of nodes that are not inS, but are descendants of the nodes in S, and

• P parent sub-graph is the induced sub-graph for the remaining nodes that areneither in S nor in C.

The S, C and P sub-graphs represent a disjoint partitioning of the nodes in T. Thefollowing properties hold for the C and P sub-graphs:

1. C is path-induced.2. P is path-induced.

14.2 Partitioning SKO Models 275

Proof:

1. Assume that there are nodes i � j � k where i,k ∈ C. Since j is a descendantof i, it must belong to either S or C. We will show that node j must necessarilybelong to C. First, since i ∈ C, by definition there is a node l ∈ S which is anancestor of node i, i.e., l� i� j.If node j ∈ S, we would have l � i � j. Since i /∈ S, this would contradict theassumption that S is path-induced. Hence, j /∈ S and j ∈ C. Therefore, C ispath-induced.

2. Assume that there are nodes i� j� k, where i,k ∈ P. We will show that node jmust necessarily be in P.If j ∈ S, then, since node k is its child, k must be either in S or C, which wouldcontradict our assumption that k ∈ P. Thus j /∈ S.If instead j ∈ C, then there must a node l ∈ S that is the ancestor of j, i.e., l� j.This would imply that l� k as well, since j� k. Since kwill then be a descendantof l ∈ S, it must belong to either S or C. This would contradict our assumptionthat k ∈ P. Hence, node j must be in P. This establishes that P is path-induced.

P contains nodes that are ancestors of the nodes in S but are not themselves inS, together with all nodes that are unrelated with any of the nodes in S. While allnodes in T belong to one of the S, C and P sub-graphs, connecting edges betweenthese sub-graphs do not belong to any of the sub-graphs.

14.2 Partitioning SKO Models

When T is a tree digraph, a path-induced sub-graph of T is in fact a path-inducedsub-forest, i.e., it is a collection of one or more disjoint path-induced sub-trees. Thus,in the partitioning described in Lemma 14.1, S, C, and P are all path-induced sub-forests. This is illustrated in Fig. 14.2. The partitioning process can be continued onthe component sub-forests to further sub-divide them into finer-grain path-inducedsub-forests, if desired.

14.2.1 Partitioning SKO Model Operators

When T is the tree digraph for an SKO model, the disjoint partitioning inducedby a path-induced sub-graph S also partitions the bodies in the multibody systeminto disjoint component multibody systems, corresponding to the S, C, and P path-induced sub-forests. Since these are sub-forests, each of the component systemshave well-defined SKO spatial operators, denoted EAS

, EAC, and EAP

, respectively.For ease of exposition, we assume that T is a canonical tree. The S, C and P

sub-graphs are then also canonical. Therefore, the EA, EAS, EAC

, and EAPSKO


T tree

S, path-inducedsub-forest

P, path-inducedparent sub-forest

S, path-inducedsub-forest

C path-inducedchild sub-forest

Fig. 14.2 Illustration of the disjoint partitioning of a T tree by a path-induced sub-forest S into parent P, and child C path-induced sub-forests

operators are all strictly lower-triangular. The system-level EA SKO operator canthen be expressed in the following block-partitioned form:

EA =

⎛

⎜

⎝

EAC0 0

BS EAS0

0 ES EAP

⎞

⎟

⎠(14.2)

The SKO operators of the individual sub-forests form the block-diagonal elements

of EA, with:

1. The EACSKO operator for C – with the smallest indices – being in the upper-left

corner.2. The EAP

SKO operator for P – with the largest indices – being in the lower-rightcorner.

3. The EASSKO operator for S being in the middle.

4. BS is a connector block, whose non-zero elements are for the parent/child edgesbetween the nodes in S and their children in C.

5. ES is also a connector block, whose non-zero elements are for the parent/childedges between the nodes in P and their children in S.

6. The lower-left block is zero because none of the nodes in C are the children ofthe nodes in P.

It is worth pointing out that the block lower-triangular structure of EA in (14.2) con-tinues to hold even under the more relaxed condition where the component systemsare not necessarily canonical systems themselves, but are only canonical with re-spect to each other. In this situation, the index of a node in C whose parent is in S isrequired to be less than that of its parent. Similarly, the index of a node in S whoseparent is in P is required to be less than that of its parent

Since the component systems are sub-forests, their corresponding SPO operatorsare well-defined, and given by:

14.2 Partitioning SKO Models 277

AC�= (I−EAC

)−1, AS�= (I−EAS

)−1, AP�= (I−EAP

)−1 (14.3)

The following lemma describes the corresponding partitioned structure of the sys-tem level SPO operator A in terms of the SPO operators of the component systems.

Lemma 14.2 Partitioning of the A SPO operator.The A SPO operator has the following partitioned structure corresponding to the

partitioned structure of EA in (14.2):

A =

⎛

⎜

⎝

AC 0 0

ASBSAC AS 0

AP(ESASBS)AC APESAS AP

⎞

⎟

⎠(14.4)

Observe that the SPO operators for the component sub-graphs are the block-diagonal elements of A.Proof: Start with the partitioned expression for EA in (14.2), and observe that

A−1 8.15

= (I−EA)14.2=

⎛

⎜

⎝

I−EAC0 0

−BS I−EAS0

0 −ES I−EAP

⎞

⎟

⎠

14.3=

⎛

⎜

⎝

A−1C 0 0

−BS A−1S 0

0 −ES A−1P

⎞

⎟

⎠

The result follows by verifying that the product of this expression for A−1, with A

in (14.4), is indeed the identity matrix.If the P sub-graph is empty, then the rows and columns for the parent sub-graph

do not exist in the partitioned structure. Similarly, if C is empty, then the corre-sponding columns and rows for the child sub-graph do not exist in the partitionedstructure.

14.2.2 Partitioning of an SKO Model

The partitioning of the tree digraph for an SKO model also induces the followingcorresponding partitioning of the system-level block-diagonalH and M operators:

H =

⎛

⎜

⎝

HC 0 00 HS 0

0 0 HP

⎞

⎟

⎠and M =

⎛

⎜

⎝

MC 0 00 MS 0

0 0 MP

⎞

⎟

⎠(14.5)


Observe that (HC,AC,MC), (HS,AS,MS), and (HP,AP,MP) define SKOmodels for the component tree-topology multibody systems associated with the C,S, and P sub-graphs, respectively.

Exercise 14.2 Mass matrix invariance of the outer sub-system.Consider an SKO model partitioned as in (14.2), but with empty C. That is, the

system is partitioned into inner, P, and outer, S, SKO models. Show that the overallSKO model mass matrix has the following partitioned structure:

M =

(

MS X

X∗ MP +Y

)

(14.6)

where

MS�= HSASMSA

∗SH

∗S, MP

�= HPAPMPA

∗PH

∗P

X�= HSASMSA

∗SE

∗SA

∗PH

∗P, Y

�= HPAPESASMSA

∗SE

∗SA

∗PH

∗P

(14.7)

Observe that MS and MP are the respective mass matrices for the S and P SKOmodels. Also, observe that the upper-right sub-block MS of M is independent ofquantities associated with the P sub-graph. This is consistent with a similar obser-vation in Sect. 4.1.4 on page 63 that the elements of the mass matrix do not dependupon the properties or generalized coordinates of inboard bodies.

14.3 SPO Operator Sparsity Structure

Researchers have used system-level matrices and operators to analyze and ex-ploit the sparsity structure of the mass matrix to develop efficient computationalalgorithms for the inverse and forward dynamics problems [23, 51, 132, 157]. Inthis section, we apply the partitioning techniques developed in Sect. 14.1 to studythe sparsity structure of the SKO and SPO operators and the mass matrix for tree-topology systems.

14.3.1 Decomposition into Serial-Chain Segments

Topologically, serial-chain systems represent the simplest examples of tree systems,and serve as useful elemental units for studying the sparsity structure of matrices fortree-topology systems. Unlike trees, all bodies in a serial-chain are related, so that,for any pair of bodies, one is necessarily an ancestor of the other.

14.3 SPO Operator Sparsity Structure 279

11

21

31

41

12

22

3213

23

33

43

14

24

34

15

25

35

45

55

Branch 1

Branch 2

Branch 3Branch 4

Branch 5

Fig. 14.3 Illustration of the decomposition of a tree-topology system into a tree of serial-chain branch segments

Since serial-chain segments are also path-induced sub-graphs, the partitioningtechnique from Sect. 14.1 can be used to successively decompose a tree system intodisjoint serial-chain branch segments. Figure 14.3 illustrates the decomposition ofa tree into branches 1 through 5, where each of the branches is a serial-chain seg-ment. With such a decomposition, the tree system can be regarded as a new tree,with one node for each of the serial-chain branch segments. Thus, the new tree ofbranches is homeomorphic to the original tree.1 The sparsity structure of the spatialoperators (e.g., SPO, mass matrix) can be inferred directly from the structure of thetree of branches. Due to their serial-chain structure, the blocks associated with in-dividual serial-chain branches are dense, with sparsity arising for branch pairs thatare unrelated, i.e., are not connected by a directed path in the tree of branches.

1 A pair of digraphs are said to be homeomorphic if they can both be obtained from a commondigraph by a sequence of adding and removing nodes (also known as subdivisions) along serialsegments [38, 183].


0

0

0

0

0 0

0

0 0

000

00 00

1

1

2

2

3

3

4

4

5

5

EA1

EA2

EA3

EA4

EA5

EA4,1 EA4,2

EA5,3 EA5,4

Branch

Fig. 14.4 Structure of the EA spatial operator for the tree topology system in Fig. 14.3

14.3.2 Sparsity Structure of the EA SKO Matrix

Assuming that the original tree is a canonical tree, Fig. 14.4 and (14.8) illustrate thedecomposition of the system-level EA SKO operator in terms of the SKO operatorsfor each of the serial-chain branch segments.

EA =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

EA1 0 0 0 0

0 EA2 0 0 00 0 EA3 0 0

EA4,1 EA4,2 0 EA4 0

0 0 EA5,3 EA5,4 EA5

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(14.8)

Eφj denotes the SKO operator for the jth branch segment. Also, when the kthbranch is a child of the jth branch, the EAj,k block denotes the non-zero connectorblock between them. All other blocks are zero. The structure of EAj,k is as follows:

EAj,k

�=

⎛

⎜

⎜

⎜

⎜

⎝

0 · · · 0 A(1j,nk)

0 · · · 0 0... · · · ...

...

0 · · · 0 0

⎞

⎟

⎟

⎟

⎟

⎠

(14.9)

14.3 SPO Operator Sparsity Structure 281

where 1j denotes the tip body on the jth branch that is the parent of the nkbase-body of the kth branch. This partitioned structure of EA is a generalizationof the partitioned structure in (14.2).

14.3.3 Sparsity Structure of the A Matrix

With φj�= (I−Eφj)

−1 denoting the SPO operator for the jth branch, the overallstructure of the system level A SPO operator for the Fig. 14.3 tree is illustrated inFig. 14.5 and (14.10):

0

0

0

0

0

0 0

000

00 00

1

1

2

2

3

3

4

4

5

5

A1

A2

A3

A4

A5

A4,1 A4,2

A5,1 A5,2 A5,3 A5,4

Branch

Fig. 14.5 Structure of the A spatial operator for the tree-topology system in Fig. 14.3

A =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

A1 0 0 0 00 A2 0 0 0

0 0 A3 0 0A4,1 A4,2 0 A4 0

A5,1 A5,2 A5,3 A5,4 A5

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(14.10)

The Aj,k blocks denote the non-zero connector block between related serial-chainsegments. All other blocks are zero. This partitioned structure of EA is a general-ization of the partitioned structure in (14.4). The general expression for the Aj,k

elements is:


Aj,k = AjEAj,iAi,k with i ∈ �(j), i� kIn the above, Ak,k ≡ Ak.

14.3.4 Sparsity Structure of the M Mass Matrix

0

0

0

0

0

0 0

0

1

1

2

2

3

3

4

4

5

5

Branch

Fig. 14.6 Structure of the mass matrix M for the tree topology system in Fig. 14.3

Based on the mass matrix decomposition discussed in Sect. 9.3.2, Fig. 14.6 il-lustrates the sparsity structure of the mass matrix for the tree-topology system inFig. 14.3, partitioned according to its branch segments. As expected, the sparsitystructure of the mass matrix mirrors the sparsity of the A SPO operator in Fig. 14.5and that of its transpose. The block-diagonal of the mass matrix contains denseblocks, one for each of the branch segments in the tree. The off-diagonal blocks arezero for unrelated segment pairs, i.e., ones where neither is the ancestor of the other.The remaining non-zero off-diagonal elements are for segment pairs that are related.This structure of the mass matrix is a generalization of the discussion on this subjectin Sect. 9.3.2. The topology dependency of the mass matrix’s sparsity structure wasinitially described in Rodriguez et al. [152]. Additional discussion on this topic canbe found in Featherstone [51].

14.4 Aggregating Sub-Graphs

In this section we study transformations of a digraph by the process of aggregatingsub-graphs into aggregated nodes.

14.4 Aggregating Sub-Graphs 283

14.4.1 Edge and Node Contractions

In graph theory, edge-contraction is referred to as the process of collapsing thenode pair for an edge, into a single node [183]. The node and edge neighbors ofthe original pair of nodes become neighbors of the new aggregated node. Thus,the parent and children nodes of either of the original nodes (not including the nodepair themselves) are the parent and children nodes of the aggregated node in thetransformed digraph.

For a tree, the result of each such edge-contraction is once again a tree, withone fewer node. The edge-contraction process can be repeated to aggregate multipleedges in a tree. Thus, aggregating a sub-tree of a tree, using edge-contraction, resultsin a digraph that is also a tree. The process of creating a tree of branches digraphfrom a tree digraph in Sect. 14.3 is an example of aggregating serial-chain segmentswithin the tree using edge-contraction.

Node-contraction is a more general concept that applies to pairs of nodes thatare not necessarily connected by an edge. The node-contraction of a pair of nodesreplaces the pair of nodes with a single aggregated node, where all the neighbor-ing nodes and edges of the original pair become neighbors of the new aggregatednode. Thus, edge-contraction is equivalent to node-contracting the node pair foran edge. Unlike edge-contractions, node-contractions generally do not preserve thetree property of a digraph. Node-contractions can be applied repeatedly to aggre-gate multiple nodes in a sub-graph. Formally, aggregation of a sub-graph is definedas the process of transforming a digraph by applying node-contraction to all thenodes in the sub-graph. This is illustrated in Fig. 14.7. It shows examples of treestransformed into multiply-connected and cyclic digraphs, following sub-graph ag-gregation. Later, we will examine the conditions under which the tree property ispreserved following sub-graph aggregation.

The following lemma shows that aggregating a sub-graph or its induced sub-graph result in the same transformed digraph.

Lemma 14.3 Aggregation of a sub-graph and its induced sub-graph.Let S denote the sub-graph of a digraph T. Then, the new digraph, TS, created

by aggregating S, is the same as the one obtained by aggregating the induced sub-graph, SI, i.e., TS ≡ TSI .Proof: S and SI contain the same nodes, while the latter contains all the edgesconnecting the nodes as well. Node-contraction of a pair of nodes removes all edgesconnecting the node-pair. Thus, aggregating S by node-contraction removes alledges connecting the nodes in S, whether or not the edges are in S itself. This im-plies that the aggregation of S or SI results in the same transformed digraph.

One implication of this lemma is that the nodes and edges deleted from thetransformed tree, after aggregating S, are precisely the nodes and edges in the SIinduced sub-graph of S.


11 23

3 44 5

66

sub-graph beingaggregated

aggregated node

11

22

33

4

55

6 6

77

8 8

99

10

11 11

12 12

sub-graphbeingaggregated

aggregatednode

11

22

3344

5 67

88

9

10 10

sub-graphbeingaggregated

a

b

c

aggregatednode

A tree transformed into a multiply-connected digraph after sub-graph aggrega-tion

A tree transformed into a multiply-connected digraph after sub-graph aggregation

A serial-chain transformed into a cyclic digraph after sub-graph aggregation

Fig. 14.7 Examples of sub-graph aggregation using node-contraction

14.4.2 Tree Preservation After Sub-Graph Aggregation

Let S denote a sub-graph of a tree digraph, T. The aggregation of S results in a newTS digraph, where all the nodes associated with the nodes in S are replaced by asingle node. We will henceforth refer to this aggregated node, as node S. Topologi-cally, all the parent nodes of the S sub-graph, denoted ℘(S), are now the parents ofnode S, and all the children nodes of the S sub-graph, denoted �(S), are now thechildren of node S. In general, TS obtained by this topological transformation willnot be a tree.

The following defines the aggregation condition for sub-graphs.

14.4 Aggregating Sub-Graphs 285

Assumption 14.1 (Aggregation Condition). A sub-graph S, of a tree digraphT, is said to satisfy the aggregation condition if:

1. S is an induced sub-graph, i.e., S ≡ SI.2. ℘(S) contains exactly one node – one that is necessarily the ancestor of all the

nodes in S.

The induced requirement for a S satisfying the aggregation condition is a mild onesince Lemma 14.3 states that aggregating a sub-graph, or its induced sub-graph, leadto the same transformed digraph. Some examples of a tree sub-graph S, satisfyingthe aggregation condition, are:

• A single node• A serial-chain segment• A sub-tree

The following lemma shows that any sub-graph of a tree satisfying the aggregationcondition is a path-induced sub-graph.

Lemma 14.4 Aggregation condition and path-induced sub-graphs.If a sub-graph S of a T tree satisfies the aggregation condition, then it is path-

induced, i.e., S ≡ SP.Proof: Since S satisfies the aggregation condition, it is an induced sub-graph.An induced sub-graph of a tree can differ from its path-induced sub-graph in thenodes and edges that lie on the paths connecting nodes in S.

First, let us consider the case where the node sets are the same. Since S is in-duced, there are no missing edges in S, and, therefore, even the set of edges areexactly the ones in the path-induced sub-graph. Hence, S and its path-induced sub-graph are the same.

Let us now consider the case when the node sets are not the same. Then, thereexists a node k in the path-induced sub-graph SP that is not in S. Node k mustlie on a path connecting a pair of nodes in S. This implies that there is a node lon this path that belongs to ℘(S). However, since k is in the interior of the path,node l is not the ancestor of all the nodes in S. This contradicts the assumption thatS satisfies the aggregation condition, which allows for only a single ancestor nodefor S. Hence, S must be a path-induced sub-graph.

The converse is not true, i.e., not all path-induced sub-graphs satisfy the aggre-gation condition. Figure 14.8 illustrates cases of path-induced sub-graphs that do,and do not, satisfy the aggregation condition.

The following lemma shows that the aggregation condition is a necessary andsufficient condition for the TS transformed digraph to be a tree.

Lemma 14.5 Tree property after sub-graph aggregation.Let S denote an induced sub-graph of a tree digraph T. Let us assume that S is ag-

gregated to create a new TS digraph with aggregated node S. Then, the aggregatedTS digraph is a tree if and only if S satisfies the aggregation condition.


singleparentnode

multipleparentnodes

Aggregationcondition satisfied

Aggregationcondition not satisfied

aggregationsub-graph


Fig. 14.8 Examples of path-induced sub-graphs that do, and do not, satisfy the aggre-gation condition

Proof:

1. First, let us assume the TS is indeed a tree, and we will show that ℘(S) mustcontain a single node. Since TS is a tree, all of its nodes, including node S, canhave at most one parent. Thus, ℘(S) can have at most one node. The S sub-graphtherefore satisfies the aggregation condition.

2. Now, let us consider the converse case, i.e., assume that ℘(S) contains a singlenode that is the ancestor for all the nodes in S. We will prove that TS must thenbe a tree. We need to show that TS is not a polytree, is not multiply-connected,and does not have directed cycles.

a. To show that TS is not a polytree, we need to focus only on node S and showthat it cannot have multiple parents. This follows directly from the assumptionthat ℘(S) contains only one node.

b. To show that TS is not multiply-connected, we need to focus only on the nodeS and show that there is no node in TS from which there is more than onedirected path to the node S. Since T is a tree, it has no multiply-connectednodes. Thus, the only way for node S to be multiply-connected is for mul-tiple paths to end at node S. This would mean that the node S has multipleparents – one from each of the paths that end on it. This would contradict theassumption that ℘(S) contains a single node.

c. Now, we show that TS cannot have directed cycles. Again, we need to focuson node S, and show that it cannot be part of a directed cycle. Assume thatsuch a cycle exists. This implies that there are nodes k, j ∈ S, and a nodei /∈S such that k� i� j. In other words, there is a path from k to j containingnode i. This would imply that there is a node, l, on the path from i to j thatis a parent node for S. Since ℘(S) contains only one node, l must be thisancestor node. This would imply that node l, and hence node i, is an ancestorof k. However we know that k is an ancestor of i. Hence, node S is not partof any directed cycles.

14.5 Transforming SKO Models Via Aggregation 287

Lemma 14.5 shows that preservation of the tree property after sub-graphaggregation requires that the sub-graph satisfy the aggregation condition. Thisresult is not surprising, once we recall that edge-contractions preserve the tree prop-erty, and that the aggregation of a path-induced sub-graph is equivalent to applyingedge-contraction to all the edges in the sub-graph,

14.4.3 The SA Aggregation Sub-Graph

The aggregation sub-graph, for a sub-graph S of a tree, is defined as the minimalsub-graph containing S that also satisfies the aggregation condition. The aggrega-tion sub-graph of a sub-graph S is denoted SA.

Lemma 14.6 Properties of SA.

1. SA is a path-induced sub-graph.2. The following containment relation holds

S ⊆ SI ⊆ SP ⊆ SA (14.11)

3. SA is the sub-forest obtained by deleting the root node from the smallest sub-treecontaining S.

Proof:

1. By definition, SA satisfies the aggregation condition and, hence, by Lemma14.4, it is path-induced.

2. By definition, S ⊆ SA. Hence, the path-induced sub-graph of S, SP, is con-tained in the path-induced sub-graph of SA, which is SA itself. This establishesthe last containment in (14.11). The rest are restatements of (14.1).

3. Since SA satisfies the aggregation condition, it has a single parent node. Thus,with the parent node added in, the new sub-graph is a sub-tree. The minimalityof the sub-tree follows from the definition of a aggregation sub-graph, which isrequired to be minimal.

While aggregation of a sub-graph S will generally not preserve the tree property,aggregating SA instead ensures that the aggregated digraph is a tree.

14.5 Transforming SKO Models Via Aggregation

14.5.1 SKO Operators After Body Aggregation

Let T be a tree digraph associated with a multibody system. We assume without lossin generality that it is canonical. Let S denote a sub-graph satisfying the aggregation


condition. By Lemma 14.5, the aggregated TS digraph, with aggregated node S, isalso a tree. Let us now develop an SKO model for the system with the TS aggre-gated tree.

To define SKO operators for TS, we first need to assign weight dimensions tothe nodes in TS. All nodes unaffected by the aggregation process inherit the weightdimensions from the nodes in T. The weight dimension for node S is defined as thesum of the weight dimensions of all the nodes in the S sub-graph, i.e.,

mS�=

∑k∈S

mk (14.12)

Since the S sub-graph satisfies the aggregation condition it is path-induced, asshown in Lemma 14.4. From Lemma 14.1, S induces a disjoint partitioning of Tinto path-induced parent P and child C sub-graphs respectively. These partitionedsub-graphs allow us to express the Eφ SKO operator for T in the partitioned formshown in (14.2):

EA =

⎛

⎜

⎝

EAC0 0

BS EAS0

0 ES EAP

⎞

⎟

⎠(14.13)

Lemma 14.7 SKO operator for TS tree.With S satisfying the aggregation condition, define the EAa

matrix using the par-titioned sub-blocks of (14.13) as follows:

EAa

�=

⎛

⎜

⎝

EAC0 0

BS 0 00 ESAS EAP

⎞

⎟

⎠(14.14)

The following facts hold for EAa:

1. EAais an SKO operator for the original tree, T.

2. EAais an SKO operator for the aggregated tree, TS.

Proof:

1. Since S satisfies the aggregation condition, ℘(S) contains a single node. Let usdenote this node as node j. Comparing the expression for EAa

in (14.14) withthat of EA in (14.13), the only term that needs further examination to establishthe SKO property is the ESAS block, which differs from the ES block. Recallthat the ES connector block contains non-zero entries for the edges connectingnodes in P to nodes in S sub-graphs. Therefore, ES has the form

ES =∑

l∈℘(S)

el∑k∈S℘(k)=l

A(l,k)e∗k = ej∑k∈S℘(k)=j

A(j,k)e∗k (14.15)


In the above equations, we have taken some notational liberties; the ex vectorsrepresent appropriate size vectors for the S and P sub-graphs, instead of thefull-sized ones for the T tree.It follows that

ESAS14.15= ej

∑k,i∈S℘(k)=j

A(j,k)e∗kAS(14.16)

The e∗kAS product is the kth row of AS. Since AS is an SPO operator, its kthrow A(k, i) entry is non-zero only when node i is a descendant of the kth node.Hence, (14.16) can be re-expressed as

ESAS14.16= ej

∑k∈S

℘(k)=j, k�i

A(j,k)A(k, i)ei = ej∑i∈Sj�i

A(j, i)e∗i (14.17)

In the last expression, j� i, denotes the condition that the jth node is an ancestor

of the ith node. From (14.17), it is clear that only the jth row of ESAS is non-zero. The only non-zero entries in this row are for nodes in S that are descendantsof the jth node.2 Thus, each column of ESAS, has at most a single non-zeroelement, and since the central block of EAa

is zero, this implies that the samecolumn has only a single non-zero entry in the full EAa

matrix as well. Thus,the single non-zero entry per column requirement for an SKO operator for T issatisfied. Hence, EAa

is an SKO operator for T.2. We have seen that EAa

is an SKO operator for T. For TS, the central rows andcolumns of EAa

correspond to the single S node in the TS tree. For EAato

be an SKO operator for TS, we need to show that it satisfies the SKO operatorstructural requirements, as represented by (8.12). That is, ESAS must be of theform ejX for some X. Equation (14.17) satisfies this requirement, and, thus, EAa

is an SKO operator for TS.

Having derived the expression for an EAaSKO operator for TS, the following

lemma derives the expression for the corresponding SPO operator for TS.

Lemma 14.8 The Aa SPO operator for the TS tree.Using the same assumptions and notation from Lemma 14.7, the SPO operator,

Aa, for TS is given by the following expression:

2 In fact all nodes in S are descendants of j and therefore the row is fully populated.


Aa�= JaA

14.4=

⎛

⎜

⎝

AC 0 0

BSAC I 0AP(ESASBS)AC APESAS AP

⎞

⎟

⎠

where Ja�=

⎛

⎜

⎝

I 0 0

0 A−1S 0

0 0 I

⎞

⎟

⎠

(14.18)

Proof: Since Lemma 14.8 established EAaas an SKO operator for the TS tree,

we need to show that Aa = (I−EAa)−1 for it to be an SPO operator for TS. Now

(I−EAa)

14.2=

⎛

⎜

⎝

I−EAC0 0

−BS I 00 −ESAS I−EAP

⎞

⎟

⎠

14.3=

⎛

⎜

⎝

A−1C 0 0

−BS I 00 −ESAS A

−1P

⎞

⎟

⎠

The result follows by verifying that the product of the above, with Aa in (14.18), isthe identity matrix.

Node S in TS contains all the bodies associated with the nodes in S. Unlikeregular rigid links, the geometry of node S is variable, and depends on the hingecoordinates of the component links within the sub-graph S. Such variable geometrybodies have been used in other dynamics modeling contexts [42,68,186]. The trans-formation of a tree digraph into a tree of serial-chain branch segments, discussed inSect. 14.3 on page 278, is an example of transforming one tree digraph into anotherby aggregating the serial-chain segment sub-graphs.

14.5.2 SKO Model for the TS Aggregated Tree

The key difference between the aggregated and the original tree is that the formertreats the set of bodies in S as a single body. The aggregation process provides away of transforming and sub-structuring SKO models for tree-topology multibodysystems into coarser SKO models. The aggregation process induces the followingpartitioning of the θ, V, f and T stacked vectors:

θ =

⎡

⎢

⎣

θC

θS

θP

⎤

⎥

⎦, V =

⎡

⎢

⎣

VC

VS

VP

⎤

⎥

⎦, f =

⎡

⎢

⎣

fC

fS

fP

⎤

⎥

⎦, T =

⎡

⎢

⎣

TC

TS

TP

⎤

⎥

⎦(14.19)

For the TS aggregated tree, the VS, θS, etc., sub-vectors of the V and θ

stacked vectors correspond to the single S aggregate link. This partitioning of the


system-level θ and V stacked vectors extends to other stacked vectors such as theCoriolis spatial accelerations vector a, the gyroscopic spatial forces vector b, and tothe other spatial operators.

Lemma 14.9 shows that the transformed system with the aggregated tree pos-sesses a well-defined SKO model, and defines the equations of motion for the model.

Lemma 14.9 SKO model for an aggregated tree.Let (H,A,M) denote an SKO model with tree digraph T. Let S be a sub-graph

of T satisfying the aggregation condition. Then the (Ha,Aa,M) spatial operatorsdefine an SKO model for the TS aggregated tree, where

Ha�= HJ−1

a14.18=

⎛

⎜

⎝

HC 0 0

0 HS 00 0 HP

⎞

⎟

⎠and HS

�= HSAS (14.20)

Ha defines the new joint map matrix for the TS aggregated tree, with HC, HS,andHP denoting the component joint map matrices as defined in (14.5). The trans-formed version of the equations of motion from (9.1) are given by:

V = A∗aH

∗a θ

α= A∗a(H∗

a θ+a), where a�= J−∗

a a =

⎡

⎢

⎣

aC

aS

aP

⎤

⎥

⎦, aS

�= A

∗SaS

f = Aa(Mα+b), where f�= Jaf =

⎡

⎢

⎣

fC

f′S

fP

⎤

⎥

⎦, f

′S

�= A

−1S fS

T =Haf

(14.21)

Thus,

T = Mθ+C (14.22)

with

M =HaAaMA∗aH

∗a ∈RN×N and C

�= HaAa(MA

∗aa+b) ∈RN (14.23)

The mass matrix expression above represents the Newton–Euler operator factoriza-tion of the mass matrix for the SKO model of the aggregated system.Proof: Lemma 14.8 established that Aa is the SPO operator for TS. With the cen-tral row and column of Ha corresponding to node S in TS, Ha is block-diagonalfor TS. Lastly, M has remained unchanged in going from T to TS. Thus, the struc-tural requirements 1 through 4 for an SKO model in Sect. 9.1.1 on page 160, aresatisfied for the TS tree, by the (Ha,Aa,M) operators.


The equations of motion in (14.21) follow by substituting the expressions forHa and Aa from (14.18) and (14.20) in the original equations of motion in 9.1 onpage 160.

Observe that, while HS is block-diagonal, HS = HSAS is no longer block-diagonal. Consequently, Ha is not block-diagonal for T and the (Ha,Aa,M) op-erators do not satisfy the structural requirements for an SKO model for the originalT. Nevertheless, the above lemma shows that these operators do satisfy the SKOmodel structural requirements for the TS tree. This allows all of the SKO formu-lation techniques and algorithms from Chap. 9, including mass matrix factorizationand inversion, to be applied to the SKO model of the aggregated tree.

Exercise 14.3 Mass matrix invariance with aggregation.Show that the mass matrix, M, and the Coriolis vector, C, of a tree-topology sys-

tem remain unchanged after sub-graph aggregation. In other words, show that theexpressions for M and C in (14.23) agree with the quantities defined in (9.3), i.e.,

M =HAMA∗H∗ =HaAaMA

∗aH

∗a

C =HA(MA∗a+b) =HaAa(MA

∗aa+b)

(14.24)

The aggregation process provides a way to develop alternative SKO models fora system. As observed earlier, the tree of serial-chain branches used in Sect. 14.3 tostudy the sparsity structure of the SPO operator and the mass matrix of a standardSKO model, is a tree obtained by aggregating all the serial branch segments. Thediscussion in this section shows how to develop the sub-structured SKO model forthis aggregated tree.

14.6 Aggregation Relationships at the Component Level

In this section, we take a closer look at the component level recursive relationshipsfor the aggregated SKO model. The parent link for the S aggregate link is denotedp, while c denotes a child body of S. Also, i denotes a body in the S sub-graph thatis a child of p, i.e., a base-body in the S sub-graph. Body j in the S sub-graph de-notes the parent of body c. This notation is illustrated in Fig. 14.9. We focus on thecomponent-level relationships involving the aggregated body and its adjacent bod-ies since these relationships are the only ones effected by the aggregation process.The relationships involving non-aggregated links alone remain unchanged.

14.6 Aggregation Relationships at the Component Level 293

i

p

c jS aggregationsub-graph

Fig. 14.9 Body indexing scheme for the S aggregation sub-graph

14.6.1 Velocity Relationships

Recall the following link-to-link spatial velocity recursive relationship from (9.10)for bodies in the T tree:

V(k) = A∗(℘(k),k)V(℘(k))+H∗(k)θ(k) (14.25)

We will use this relationship to derive velocity relationships for the S aggregatedbody and its adjacent bodies in the TS tree. First, let us develop a recursive rela-tionship for VS. Assembling (14.25) for all the links in S, we obtain the followingrelationship for VS

VS = E∗ASVS +E∗SV(p)+H∗

SθS where ES�= [0, 0, · · · A(p, i)] (14.26)

This implicit relationship for VS involves quantities associated with nodes in theTS aggregated tree instead of nodes in the T spanning tree. ES is the connectorblock matrix for the coupling between the S aggregated body and its p parent body.While the (14.26) expression for ES shows only a single A(p, i) non-zero entry,more generally, it will have a non-zero entry for each base-body in S at locationscorresponding to the indices for the base-bodies. Since EAS

is an SKO operator, we

can use the AS�= (I−EAS

)−1 expression from (14.3) to convert the implicit formof (14.26) into the following explicit expression:

VS14.3= A

∗SE

∗SV(p)+A

∗SH

∗SθS

14.20= A

∗SE

∗SV(p)+H∗

S θS (14.27)

The form of (14.27) is similar to (14.25). It defines how the spatial velocity of theparent body p couples into the aggregated body S. From this expression, we inferthat ESAS is the A(p,S) SKO weight matrix associated with the S and p adjacentbody pair in TS, i.e.,

A(p,S) ≡ ESAS (14.28)


Observe that the ESAS weight matrix also appears in the partitioned expression for

the EA aggregated SKO operator in (14.14).The V(c) spatial velocity of the child link c depends on the spatial velocity of its

parent link j in the S sub-graph. The (14.25) relationship for link c can be restatedas:

V(c) = B∗cVS +H∗(c)θ(c) where Bc

�=

⎡

⎢

⎢

⎢

⎢

⎣

A(j,c)

0...

0

⎤

⎥

⎥

⎥

⎥

⎦

(14.29)

The form of this new relationship is similar to (14.25) and defines how the velocitiesof the S parent body couple into the child body c. Equation (14.29) involves quanti-ties associated with nodes in the TS aggregated tree instead of quantities associatedwith nodes in the T spanning tree. Bc is the connector block matrix for the couplingbetween the c body and its parent body S. From this expression, we infer that Bc isthe A(S,c) SKO weight matrix associated with the c and S adjacent pair of bodies,i.e.,

A(S,c) ≡ Bc (14.30)

There is one Bc matrix for each child body c in �(S). The Bc matrices for all thechildren bodies make up the columns of the BS matrix in the partitioned expressionfor EA in (14.14).

Gathering together the expressions in (14.27) and (14.29), the spatial velocityrelationship between the S aggregate link and its adjacent links p and c are asfollows:

VS14.27.14.28

= A∗(p,S)V(p)+H∗

SθS

V(c)14.29,14.30

= A∗(S,c)VS +H∗(c)θ(c) (14.31)

14.6.2 Acceleration Relationships

Now recall the following link-to-link spatial acceleration relationship from (9.10)for bodies in the T tree:

α(k) = A∗(℘(k),k)α(℘(k))+H∗(k)θ(k)+a(k) (14.32)

Assembling (14.32) for all the links in S, we obtain the following implicit relation-ship for αS:

αS = E∗ASαS +E∗Sα(p)+H∗

SθS +aS

Once again using the AS�= (I−EAS

)−1 expression from (14.3), we can convertthe implicit form above into the following explicit expression:

14.6 Aggregation Relationships at the Component Level 295

αS14.3= A

∗SE

∗Sα(p)+A

∗SH

∗SθS +A

∗SaS

14.21,14.20,14.28= A

∗(p,S)α(p)+H∗S θS +aS

(14.33)

Similarly, the spatial acceleration relationship between body S and its child body ctakes the form:

α(c) = B∗cα(S)+H∗(c)θ(c)+a(c)

14.30= A

∗(S,c)α(S)+H∗(c)θ(c)+a(c)

(14.34)

Both (14.33) and (14.34) have the familiar recursive form of (14.32), except that

they are for the TS aggregated tree instead of the original T tree.

14.6.3 Force Relationships

We now examine the transformed spatial force expressions for the aggregated body.For this, we need the spatial inertia term,MS, for the aggregate link.MS is simplythe partitioned sub-block of the block-diagonal M corresponding to the S aggre-gated body in TS. The link-to-link spatial force relationships for bodies in the Ttree from (9.10) are as follows:

f(k) =∑

∀j∈�(k)

A(k, j)f(j)+M(k)α(k)+b(k)

T(k) =H(k)f(k)

(14.35)

Assembling these expressions for all the links in S, we obtain the following rela-tionships for fS:

fS14.35= EAS

fS +∑

∀c∈�(S)

Bcf(c)+MSαS +bS

14.30= EAS

fS +∑

∀c∈�(S)

A(S,c)f(c)+MSαS +bS

TS =HSfS

Using AS�= (I−EAS

)−1 leads to the following explicit form:


(I−EAS)fS =

∑∀c∈�(S)


⇒ f′S

14.21=

∑∀c∈�(S)


TS14.21= HSASf

′S

14.20= HSf

′S

(14.36)

Similarly, the spatial force relationship between the S and p bodies can be expressedas:

f(p)14.35= ESfS +M(p)α(p)+b(p)

14.36,14.21= ESASf

′S +M(p)α(p)+b(p)

14.28= A(p,S)f

′S +M(p)α(p)+b(p)

T(p) =H(p)f(p)

(14.37)

Taken together, (14.36) and (14.37) define the spatial force relationships involvingthe aggregated body and its adjacent links. These expressions have the recursivestructure of (14.35). They are component-level versions of the equivalent operatorexpressions for the SKO model in (14.21). They define the altered steps for aggre-gated bodies in the computational algorithms derived from the SKO model formu-lation in Chap. 9.

Chapter 15Constraint Embedding

In this chapter, we study a constraint embedding approach [74] that allows thedevelopment of SKO models for closed-chain multibody systems. Chapter 11 dis-cussed the dynamics of closed-chain multibody systems and described the aug-mented dynamics approach, where the system is decomposed into a spanning treeSKO model subject to cut-edge constraints. Although this approach is able to exploitthe SKO techniques to a degree, it suffers from the use of non-minimal general-ized coordinates. While the alternate projected dynamics formulation uses minimalgeneralized coordinates, it no longer satisfies the requirements for an SKO model.The constraint embedding process uses the sub-graph aggregation process to workaround these limitations to develop SKO models for closed-chain systems.

15.1 Constraint Embedding Strategy

The constraint embedding strategy is to transform the non-tree closed-chain systeminto an SKO model. The approach is to isolate non-tree sub-graphs and excise themusing aggregation to transform the system digraph into a tree. Once the transforma-tion is complete, an SKO model is developed for the transformed tree. The constraintembedding strategy involves the following steps:

1. Decompose the non-tree digraph for the system into a spanning tree, T, and a col-lection of cut-edges for the constraints. The set of cut-edges is usually not unique.Develop an SKO model for the system associated with the spanning tree, T.

2. For each cut-edge, identify the aggregation sub-graph, S, for the sub-graph con-sisting of the node pair for the cut-edge. Figure 15.1 illustrates minimal and non-minimal choices for an aggregation sub-graph for a system with a loop constraint.Based on Lemma 14.6, a procedure for creating this S aggregation sub-graph isis as follows:

a. Identify the smallest sub-tree that contains the nodes in the cut-edge.b. Remove the root node from this sub-tree to obtain the aggregation sub-

graph S.


298 15 Constraint Embedding

parentroot

parentroot

cutedge

cutedge

Minimalaggregation loop

Non-minimalaggregation loop

aggregationsub-graph aggregation

sub-graph

Fig. 15.1 Minimal, and non-minimal, choices for a sub-graph that is a an aggregationsub-graph for the cut-edge nodes sub-graph

3. Aggregate S, and use Lemma 14.9 to derive an SKO model for the TS

aggregated tree.4. By construction, the nodes for each cut-edge belong to the internal S sub-graph

of the aggregated body. Moreover, by Exercise 14.1, any directed cycles andmultiply-connected paths associated with the cut-edge belong to S, since thesub-graph satisfies the aggregation condition and is therefore path-induced. Con-sequently, the cut-edge constraints can be defined purely in terms of the state ofthe nodes within the S sub-graph, independent of the rest of the nodes in the sys-tem! Once all the cut-edges have been processed, we are left with an aggregatedtree containing a set of aggregated nodes, where the sub-graph of each aggregatednode has one or more cut-edge constraints internal to it. Figure 15.2 illustratesthe transformation of the original system digraph into a tree with aggregate nodeshaving internal cut-edge constraints. The next step is to eliminate the constraintfrom the sub-graph of each aggregated node. This process is described in detailin Sect. 15.1.1.

At the conclusion of the constrained embedding process, all of the constraints areabsorbed into the aggregated links and subsequently eliminated from the multibodydigraph. An SKO model, with minimal coordinates (satisfying the constraints), cannow be developed as shown in Lemma 15.2. Being an SKO model, all SKO formu-lation techniques and algorithms can be applied.

Figure 15.3 illustrates the following additional examples of aggregation sub-graphs for non-tree digraphs:

1. This example contains a pair of cut-edges. Two possible choices of aggregationsub-graphs are shown. One option contains a single aggregation sub-graph withboth cut-edges, while the second shows a pair of sub-graphs, each with a singlecut-edge. Even though the latter option results in an aggregated tree with an extranode, it is the slightly more preferable option since the aggregation sub-graphsare smaller.

15.1 Constraint Embedding Strategy 299

cut

Original digraph Transformed tree digraphwith aggregated link

aggregatedlink’s node


Fig. 15.2 Transformation of the original digraph of the system into a tree using sub-graph aggregation

2. In this example, the cut-edge is between nodes on disconnected systems. In thiscase, the aggregation sub-graph contains all the ancestor nodes of the cut-edgenodes. The aggregated node is a root node for the SKO-forest of the aggregatedtree.

3. This example shows a digraph in which a pair of cut-edges share a node.In this case, the aggregated sub-graph includes the nodes from both cut-edges,and, unlike example (1), there is no split option possibility for the aggregationsub-graph.

15.1.1 Embedding Constraint Sub-Graphs

In this section, we describe the process for eliminating the constraints from theaggregation sub-graphs required in step 4 of the constraint embedding strategy inthe previous section. We focus our attention on a single constraint cut-edge in thesystem. The process can be applied repeatedly when there are multiple cut-edgeconstraints. Let us assume that steps 1–3 in Sect. 15.1 have been completed to obtainan aggregated body, with sub-graph S satisfying the aggregation condition. The Saggregation sub-graph has the cut-edge constraints. One consequence of the cut-edge constraints is that the effective degrees of freedom in S is less than the totalhinge degrees of freedom in S.

We adopt the body indexing scheme from Sect. 14.6 on page 292 for sub-graphaggregation. The parent link for the S aggregated link is denoted p, while c denotesa child body of S. Also, i denotes a body in the S sub-graph that is a child of p,i.e., a base-body in the S sub-graph. Body j in S sub-graph denotes the parent ofbody c. This indexing scheme for S is illustrated in Fig. 15.4.

Due to the cut-edge internal constraints on the S, the elements of θS are notindependent, but are instead coupled by the constraint. At the coordinate level, theconstraint often takes the form of a nonlinear algebraic relationship. However, at the


(1) Pair of cut-edges in a digraph with single and split aggregation sub-graph options

(2) A cut-edge betweena pair of trees

(3) Pair of cut-edges withcombined aggregation sub-graph

Fig. 15.3 Examples of valid aggregation sub-graphs. For the same digraph in (1), thesplit sub-graphs are preferable to the combined single sub-graph since the sub-graphsare smaller. Example (2), has a cut-edge across independent trees. In Example (3),a pair of cut-edges share a node and, hence, they must be in the same aggregationsub-graph

velocity level, the generalized velocities in the system are subject to a linear, config-uration dependent constraint. Per our projected dynamics discussion in Sect. 11.3,it is possible to partition the θS generalized velocities of the sub-graph into dis-joint sets of independent and dependent generalized velocities. The independent setis denoted θRS. They can be used to obtain the dependent generalized velocitiessatisfying the cut-edge constraints on the sub-graph. In other words, there exists a(configuration dependent) mapping XS such that

θS = XS θRS (15.1)

In deriving (15.1) we have assumed for simplicity that the constraint is catastaticso that the particular solution of (11.33) is zero. When the constraint sub-graphsare isolated and of moderate size, the constraint elimination process is especiallystraightforward and computationally inexpensive. Examples of such local loops in-

15.1 Constraint Embedding Strategy 301

i i

pp

c c

j jcut-edge

parent

child

S aggregationsub-graph

Fig. 15.4 Body indexing scheme for the S aggregation sub-graph with its cut-edge

clude constraints associated with geared motors, 4-bar linkages, wishbone suspen-sions, and differentials, etc.

Using the partitioned structure induced by the S sub-graph, as discussed inSect. 14.6, define

HRa�=

⎛

⎜

⎝

I 0 0

0 X∗S 0

0 0 I

⎞

⎟

⎠Ha

14.20=

⎛

⎜

⎝

HC 0 0

0 HRS 00 0 HP

⎞

⎟

⎠(15.2)

with

HRS�= X∗

SHS14.20= X∗

SHSAS and θR�=

⎡

⎢

⎣

θC

θRS

θP

⎤

⎥

⎦(15.3)

θR denotes the minimal size, system-level, generalized velocity stacked vector con-taining the independent generalized velocities for the constrained system. Observefrom (15.1) and (15.2) that

H∗Ra θR =H∗

a θ (15.4)

The following lemma establishes a relationship between the sequence of spatialoperators we have introduced during the constraint embedding process.

Lemma 15.1 Velocity relationships with constraint embedding.We have the following identities for V:

V = A∗H∗ θ = A

∗aH

∗a θ = A

∗aH

∗Ra θR (15.5)

Proof: The definitions of Aa and Ha in (14.18) and (14.20) lead to the A∗H∗ =A∗aH

∗a identity and the first equality in (15.5). The latter part follows from the use

of (15.4).


The last equality in (15.5) depends on only the independent generalized veloci-ties θR for the system. It effectively eliminates the cut-edge constraints on the sub-graph, since independent generalized velocities are minimal and always satisfy theconstraint.

The following lemma develops an SKO model for the closed-chain system basedon the constraint embedding approach.

Lemma 15.2 Constraint embedding SKO model.With θR serving as the minimal size generalized velocities, and the TS aggre-

gated tree as the system digraph, the spatial operators (HRa,Aa,M) define anSKO model for the closed-chain system. The equations of motion are as follows:

V = A∗aH

∗Ra θR

α= A∗a(H∗

Ra θR+a ′), where a ′ �=

⎡

⎢

⎣

aC

a ′S

aP

⎤

⎥

⎦, a ′

S�= aS +H∗

S XS θRS

f = Aa(Mα+b)

TR =HRaf

(15.6)

Thus,

TR = Mr θR+Cr (15.7)

where

Mr =HRaAaMA∗aH

∗Ra and Cr

�= HRaAa(MA

∗aa ′ +b) (15.8)

The mass matrix expression above represents the Newton–Euler operator expressionfor the SKO model mass matrix for the constrained system.Proof: By construction, the S sub-graph satisfies the aggregation condition.Lemma 14.9 defines an SKO model for the TS aggregated tree with equations ofmotion defined as follows in (14.21):

V = A∗aH

∗a θ α= A

∗a(H∗

a θ+a)

f = Aa(Mα+b) T =Haf(15.9)

In addition, the θR generalized coordinates are independent and eliminate the cut-edge constraint within S. Since the (Ha,Aa,M) spatial operators satisfy the re-quirements for an SKO model for the TS tree digraph, replacingHa byHRa leavesthe required block-diagona l property intact, implying that (HRa,Aa,M) also sat-isfy the SKO model structural requirements.

The first equation in (15.6) follows directly from the use of (15.5) in the firstequation in (15.9).

15.2 Examples of Constraint Embedding 303

Differentiating (15.1) we have

θS = XS θRS + XS θRS (15.10)

Using this in (14.33) leads to

αS14.33= A

∗(p,S)α(p)+H∗S θS +aS

15.10= A

∗(p,S)α(p)+H∗RS θRS +aS +H∗

SXSθRS

15.6= A

∗(p,S)α(p)+H∗RS θRS +a ′

S (15.11)

Using this the expression for α in (15.9) leads to the corresponding expression in(15.6).

The generalized forces vector for the aggregated link with the reduced general-ized velocity coordinates, TRS, is defined as

TRS�= X∗

STS15.3= HRSf

′S

(15.12)

Together these establish all the expressions in (15.6). Equation (15.8) is a simpleassembly of these component relationships.

TR denotes the minimal size, system-level generalized force stacked vector withstructure similar to that of θR. Similarly, the a ′ stacked vector is a modified versionof the a Coriolis acceleration stacked vector with the a ′

S term for body S.This lemma completes the conversion of the original constrained graph multi-

body model into an unconstrained tree-topology model through constraint embed-ding. Not only have the constraints been eliminated, but we also have an SKO modelfor the system and all the SKO techniques and algorithms can be applied.

The Mr expression in (15.8) corresponds to the reduced mass matrix derived inSect. 11.3 on page 221 using the projected approach for constrained dynamics. Incontrast to the projected dynamics approach, the (15.8) expression is an Newton–Euler operator factorization for an SKO model for the system.

15.2 Examples of Constraint Embedding

There are two important classes of constraints to consider:

1. Direct joint-level constraints: In this case, the constraints are expressed as adirect (often constant) algebraic relationship between joint angles (e.g., gear-box,differential suspensions).

2. Loop constraints: In this case, configuration dependent, loop constraints involvenodes on different bodies in the system (e.g., four-bar linkages).

The following sections describe examples for these constraint types. Section15.2.1 describes the direct joint-level constraint for a geared link. Sections 15.2.2and 15.2.3 examine loop constraints for a planar four-bar linkage with differentcut-edge choices. We assume that the bodies in these systems are rigid links, and


therefore we switch to the Eφ and φ notation in place of the abstract EA and A

notation.

15.2.1 Geared Links

As discussed in Chap. 12, the motor is mounted on an inboard link driving the outerlink for a geared link as shown in Fig. 15.5. This creates a closure constraint betweenthe three bodies. The S aggregation sub-graph consists of the (k− 1)th motor andits driven (k− 1)th link body, with the kth driven link serving as the parent bodyfor the sub-graph. Using the notation from Chap. 12, the constraint embedding termsfor geared joints are given by:

kth

hinge

(k−1)th

hinge

kth link

(k+1)th

link

(k−1)th link

(k−1)th motor

hl(k)

hl(k−1)

Fig. 15.5 An internal loop created by a link mounted motor and gear mechanism

θS =

[

θm(k− 1)

θl(k− 1)

]

∈R2, θRS = θl(k− 1) ∈R1

XS =

[

μG(k− 1)

1

]

∈R2×1

XS = 0, EφS = 0 ∈R12×12, φS = I ∈R12×12

(15.13)

Observe that XS is constant and configuration independent. The constraint-embedding approach for geared systems provides an alternative to the onedescribed in Chap. 12. At the component level, the resulting equations of motion

15.2 Examples of Constraint Embedding 305

have identical form. Thus, the Chap. 12 formulation can be regarded as a specialcase of constraint embedding.

15.2.2 Planar Four-Bar Linkage System (Terminal Cut)

Now consider a four-bar linkage within a system, as illustrated in Fig. 15.6. The ag-gregation sub-graph consists of links j, k, and l, with the root link being link p.We make a terminal cut at the hinge joining links p and l, to convert the sub-graphinto a tree-topology system. The child link c is connected via a hinge to link j.

j

k

l

c

p

cut-edgeS aggregationsub-graph

Fig. 15.6 A four-bar mechanism with a terminal cut

The constraint embedding terms for the four-bar linkage with the terminal cut are asfollows:

ES =[

0, 0, φ(p, j)] ∈R6×18,

φS =

⎛

⎜

⎝

I 0 0φ(k, l) I 0

φ(j, l) φ(j,k) I

⎞

⎟

⎠∈R18×18, XS =

⎡

⎢

⎣

rl

rk

1

⎤

⎥

⎦∈R3×1

EφS =

⎛

⎜

⎝

0 0 0

φ(k, l) 0 00 φ(j,k) 0

⎞

⎟

⎠∈R18×18, BS =

⎡

⎢

⎣

0

φ(j,c)

0

⎤

⎥

⎦∈R18×6

(15.14)

In the above, rl and rk are configuration dependent parameters that characterizethe ratio of the hinge velocities of the j, k, and l hinges within the planar four-barlinkage. Thus, XS is configuration dependent and not constant.


15.2.3 Planar Four-Bar Linkage System (Internal Cut)

We once again consider the same four-bar linkage. This time an internal cut is madeat the hinge joining links k and l. This converts the sub-graph into a tree-topologysystem, as shown in Fig. 15.7. Hinges j and l connect links j and l to the p parentlink. Observe that S is now a forest of two sub-trees, unlike the terminal cut-edge

j

k

l

c

p

cut-edge

S aggregationsub-graph

Fig. 15.7 A four-bar mechanism with an internal cut

choice in the previous section. One of the trees contains just the body l, while theother contains bodies j and k. The constraint embedding terms for the four-bar link-age with internal cut are as follows:

EφS =

⎛

⎜

⎝

0 0 0

0 0 0

0 φ(j,k) 0

⎞

⎟

⎠∈R18×18

φS =

⎛

⎜

⎝

I 0 0

0 I 00 φ(j,k) I

⎞

⎟

⎠∈R18×18, XS =

⎡

⎢

⎣

rl

rk

1

⎤

⎥

⎦∈R3×1

ES =[

φ(p, l), 0, φ(p, j)] ∈R6×18, BS =

⎡

⎢

⎣

0

φ(j,c)

0

⎤

⎥

⎦∈R18×6

(15.15)

In comparison to the earlier terminal cut example, the S digraph and the EφS andφS matrices are simpler here. On the other hand, the ES matrix is more complex.ES contains two non-zero elements, one for each of the base-bodies in the S sub-graph.

15.3 Recursive AB Forward Dynamics 307

15.3 Recursive AB Forward Dynamics

Lemma 15.2 shows that the constraint embedding process leads to an SKO modelfor the closed-chain system. We now derive the Innovations factorization for thesystem mass matrix and use it to develop the AB forward dynamics algorithms forthe closed-chain system.

15.3.1 Articulated Body Inertias for the Aggregated System

First, we describe the articulated body inertia steps from the Riccati equation, (9.33)on page 175, for the aggregated link S and its adjacent links:

P+(c) = τ(c)P(c)

PS =∑

∀c∈�(S)

A(S,c)P+(c)A∗(S,c)+MS

DS =HRSPSH∗RS

GS = PSH∗RSD−1

S

τS = GSHRS

P+S = PS −τSPS

P(p) = A(p,S)P+SA

∗(p,S)+M(p)

(15.16)

Recall that (14.28) and (14.30) define the following expressions for A(p,S) andA(S,c).

A(p,S) = ESAS and A(S,c) = Bc

DS can be re-expressed as

DS15.3= X∗

S

(

HSASPSA∗SH

∗S

)

XS = X∗SMSXS

where MS�= HSASPSA

∗SH

∗S

(15.17)

MS has the structure of a mass matrix for the S sub-graph. The primary differencefrom the true mass matrix of S is that the central inertia term is PS, instead of theusual MS spatial inertia term. PS includes the articulated body contribution fromthe descendants of the S body. DS is thus the projection of this S mass matrix ontoits independent degrees of freedom.


15.3.2 Mass Matrix Factorization and Inversion

The Riccati equation solution, and the associated articulated body inertia quantitiesin (15.16), together with Lemma 9.11 on page 179, lead to the following Innova-tions factorization and inversion expressions for the mass matrix of the transformedsystem:

Mr15.8= HRaAaMA

∗aH

∗Ra

= [I+HRaAaK]D[I+HRaAaK]∗

[I+HRaAaK]−1 = I−HRaψaK

M−1r = [I−HRaψaK]∗D−1[I−HRaψaK]

(15.18)

The ψa, D, and K operators are those associated with the Riccati equation solu-tion in (15.16). Continuing on, the explicit expression for the reduced generalizedexpressions from Lemma 9.12 on page 180 takes the form:

θR = [I−HRaψaK]∗D−1[TR−HRaψa(KTR+Pa ′ +b)]

−K∗ψ∗aa ′ (15.19)

Thus, the SKO model for the system has led to an explicit expression for the θRminimal generalized acceleration vector for the closed-chain system.

15.3.3 AB Forward Dynamics Algorithm

Equation (15.19) can be converted into the AB recursive forward dynamics algo-rithm as described in Algorithm 9.5 on page 182. The overall structure of the ABforward dynamics algorithm remains unchanged, with differences only at the stepsinvolving the aggregated body and its neighbors.

The tip-to-base recursion steps from body c to the aggregated link S have thefollowing form:

z+(c) = z(c)+G(c)ε(c)

zS =∑

∀c∈�(S)

A(S,c)z+(c)+bS +PSa ′S

εS = TRS −HRSzS

νS = D−1S εS (15.20)

The tip-to-base recursion steps from body S to body p are as follows:

15.4 Computing XS and XS 309

z+S = zS +GSεS

z(p) = A(p,S)z+S +b(p)+P(p)a(p)

ε(p) = T(p)−H(p)z(p)

ν(p) = D−1(p)ε(p) (15.21)

The base-to-tip accelerations sweep steps are also altered for the aggregated link.The steps from body p to the aggregated body S are as follows:

α+S = A

∗(p,S)α(p)

θRS = νS −G∗Sα

+S

θS = XS θRS + XS θRS

αS = α+S +H∗

RS θRS +a ′S (15.22)

The steps from the aggregated body S to body c are as follows:

α+(c) = A∗(S,c)αS

θ(c) = ν(c)−G∗(c)α+(c)

α(c) = α+(c)+H∗(c)θ(c)+a(c) (15.23)

The most computationally expensive part of these steps is the evaluation and inver-sion of the DS symmetric, positive definite matrix in (15.16). Its size is the numberof independent degrees of freedom for the aggregated link. Thus, the computationalcost of the AB algorithm is no longer linear in the number of independent degreesof freedom for the aggregated links, but, instead, is (in the worst case) quadratic inthe total degrees of freedom in the S aggregation sub-graph, and cubic in the num-ber of independent degrees of freedom in the S sub-graph. These additional costs,however, are modest when the loops are of moderate size.

15.4 Computing XS and XS

XS maps the independent generalized velocities to the full generalized velocitiesof S. For the case of direct joint-level constraints, it is straightforward to select theindependent coordinates subset, and XS is then simply the gradient of the map-ping from the independent to the full generalized coordinates. For direct joint-levelconstraints, XS is often constant, and XS is consequently zero. In the followingsections, we look at the less trivial case of loop constraints, and derive expres-sions for the configuration dependent XS and its time-derivative XS needed in(15.1) and (15.6).

For loop constraints, we have an algebraic constraint on the relative velocities ofa pair of physical closure nodes in the sub-graph. Denoting a representative pair of


closure nodes as o and p, such a constraint can be expressed as

0 = A(Vo−Vp)3.53= A(Jo−Jp)θS

= A[

Jo1 −Jp1, Jo2 −Jp2]

[

θuS

θRS

]

= [Y1, Y2]

[

θuS

θRS

]

(15.24)

Here, A denotes the constraint on the relative spatial velocities between this pair ofclosure nodes, and Jo, Jp denote sub-graph Jacobians relating the generalized ve-locities of the sub-graph to the spatial velocities at the o and p closure nodes. θuS

is the complement of the θRS sub-vector in θS and represents the dependent gener-alized velocity coordinates. Jo1, etc., represent sub-blocks within the correspondingpartitioned Jacobians. Y1 and Y2 are defined as

Y1�= A(Jo1 −Jp1) and Y2

�= A(Jo2 −Jp2) (15.25)

In (15.24), the partitioning is chosen such that Y1 is square and full rank, and thusinvertible. It follows that:

θuS = −Y−11 Y2 θRS =⇒ XS =

[

−Y−11 Y2

I

]

(15.26)

This explicit expression for XS for the constraint loop is similar to the one in Ex-ercise 11.4 on page 223. The following exercise derives an expression for XS forsystems with loop constraints.

Exercise 15.1 Expression for XS.Show that

XS =

[

−Y−11 YXS

0

]

and XS θRS =

[

−Y−11 Y θS

0

]

(15.27)

where Y�= [Y1, Y2].

Equation (15.27) requires expressions for Y1 and Y2. From (15.25), these deriva-tives require A and the time derivatives of the Jo1, etc., blocks. The expression forA is usually straightforward to obtain from the inter-node constraint expression, andhence, we focus on the time derivative of the Jacobian blocks. The kth column ofJo1 is of the form φ∗(Ok,o)H∗(k), where Ok denotes the frame at the kth hingein θS, and H∗(k) is the corresponding joint map matrix for the hinge. This deriva-tive is a simple expression involving the relative linear and angular velocity of the oclosure node with respect to the Ok closure node.

15.5 Generalization to Multiple Branches and Cut-Edges 311

15.5 Generalization to Multiple Branches and Cut-Edges

We summarize extensions for applying the constraint embedding technique to themore general multibody system context.

Multiple cut-edges: Our development of the component-level constraint embed-ding expression focused on the case of a single aggregation sub-graph duringthe constraint embedding process. More generally, systems will have multipleaggregation sub-graphs from multiple cut-edge constraints for the system. In thiscase, the system will contain multiple aggregated links, potentially with multi-ple constraints embedded within each aggregated link. The structural form of theequations of motion remains the same. The key change to the algorithms is thataggregated link steps need to be executed when an aggregated link is encoun-tered, and regular steps are executed otherwise.

Multiple S child bodies: Aggregated bodies can also have multiple child bodies.For this case, BS is a multi-column matrix, with one column for each child body.Each column has a single non-zero “A(j,c)” entry corresponding to the parentbody in the S sub-graph and the child body. The AB forward dynamics algorithmhas the standard gather/scatter structure for tree-topology algorithms. Thus, thetip-to-base sweeps gather and accumulate the results from all the branches atthe aggregated link. The base-to-tip sweeps scatter the results onto the childrenbranches of the aggregated link.

Multiple S base-bodies: While the (14.26) expression for ES shows only asingle A(p, i) non-zero entry, more generally, when the sub-graph has multiplebase-bodies,ES will have a non-zero entry for each base-body at locations corre-sponding to the indices for the base-bodies. This is illustrated in the expressionsin (15.15) for the four-bar linkage example.

Closure constraint with the inertial frame: Another situation that we have notexplicitly addressed is that of a closure constraint between a body node and theinertial frame. In this case, node p in ℘(S) is the inertial frame. The aggregatedlink in this case is a base-body attached directly to the inertial frame.

Joint-level constraints involving multiple hinges: It is possible to have the situ-ation where a direct joint-level velocity constraint involves more than two hinges.Instead of a single closure-constraint edge, multiple cut-edges that span all thehinges involved in the constraint must be included in the system digraph beforedetermining the aggregation sub-graph.

Singular constraint maps: Especially in the case of loop constraints, it is pos-sible for the XS matrix to lose rank at specific singular configurations of thesub-graph. The number of independent generalized velocities decreases at suchsingular configurations. Since the invertibility of DS depends on XS being full-rank, it is important to monitor the rank of XS and to adjust its size near, and at,singular configurations.

Chapter 16Under-Actuated Systems

Techniques for the kinematics, dynamics and control of robotic systems largelyfocus on fully-actuated manipulators. Every degree of freedom is an active degreeof freedom for these manipulators. That is, for each degree of freedom, there is anindependent generalized force that can be applied by a control actuator. However,important applications involve manipulators with passive degrees of freedom, i.e.,degrees of freedom with no corresponding control actuators. A passive degree offreedom can arise by design, due to actuator failure, during manipulation of artic-ulatable objects or due to modes of operation that avoid the use of some actuators[7]. We refer to manipulators with passive degrees of freedom as under-actuatedmanipulators, and for these systems the number of available control actuators (or,more specifically, the number of independent generalized forces) is less than thenumber of degrees of freedom. Some examples of under-actuated manipulators aredescribed below:

1. Free-flying space manipulators possess six degrees of freedom for the base-body,in addition to the manipulator hinge degrees of freedom. The six base-body de-grees of freedom are controlled by an attitude and translation control system,while the manipulator motion is controlled by actuators at the hinges. The ma-nipulator is sometimes operated so as to minimize base-body disturbances andconserve fuel. In this mode of operation, the six base-body degrees of freedomare passive, while the manipulator hinge degrees of freedom are active degreesof freedom.

2. The improved dexterity and maneuverability provided by additional degrees offreedom has motivated the study of hyper-redundant and snake-like robots. It hasbeen proposed that the mass of hyper-redundant manipulators can be reducedby including actuators at only some of the hinges while keeping the remaininghinges passive [18, 39].

3. Flexible-link manipulators are inherently under-actuated. In addition to the hingedegrees of freedom, these manipulators possess deformation degrees of freedomfrom link flexibility. While careful structural analysis can provide good modelsfor the elastic forces, these generalized forces cannot be directly controlled. As aresult, the deformation degrees of freedom represent passive degrees of freedom.


314 16 Under-Actuated Systems

4. Actuator failure can convert an active hinge into a passive one. In the face ofactuator failures, some degree of fault-tolerant control is highly desirable forrobots in remote or hazardous environments. This requires the control of anunder-actuated manipulator.

5. During multi-arm manipulation of task objects, the degrees of freedom associatedwith loose grasp contacts (e.g. rolling contacts) or internal degrees of freedom oftask objects (e.g., shears, plungers) are passive degrees of freedom.

6. Fuel slosh has a significant impact on the dynamics of space vehicles. The com-plex models for fuel slosh are typically approximated to first order by pendulummodels. These pendulum degrees of freedom represent passive degrees of free-dom.

Research on these topics has resulted in the development of useful, though largelyapplication-specific, techniques for the analysis and control of these under-actuatedsystems. The extensibility of these techniques to other types of under-actuated ma-nipulators is not always obvious. For instance, most analysis of free-flying space-robots relies extensively on the non-holonomic constraint arising from the conser-vation of linear and angular momenta for these manipulators. These techniques can-not be applied to under-actuated systems such as hyper-redundant manipulators orflexible-link manipulators, for whom such momentum constraints do not hold. Thischapter develops elements of a general approach for the kinematics and dynamicsof under-actuated manipulators [78, 80].

16.1 Modeling of Under-Actuated Manipulators

While the only fundamental requirement is that the under-actuated system have anSKO model, for the sake of exposition, we assume that the system is a canonicalrigid-link serial-chain manipulator. There is no loss in generality since the ideas de-veloped here easily extend to general SKO models using the approach from Chap. 9.

In the under-actuated system context, we use the term active degree of freedomfor a manipulator degree of freedom associated with a control actuator. Conversely,a passive degree of freedom is a manipulator degree of freedom with no controlactuator. If the active degrees of freedom represent the control inputs and outputs forthe system, then the passive degrees of freedom represent the “internal dynamics”of the system. Due to the presence of friction, stiffness, and other such effects, thegeneralized force associated with a passive degree of freedom can be non-zero.

Typically, the component degrees of freedom of a multiple degree of freedomhinge are either all active or all passive. In the former case the hinge is denoted anactive hinge and in the latter, a passive hinge. It is possible to have hinges witha mix of active and passive component degrees of freedom from events such asactuator failure. However, for modeling purposes, such multiple degree of freedomhinges can be decomposed into equivalent combination of active and passive hinges.In this chapter, without loss in generality, we assume that a manipulator model with

16.1 Modeling of Under-Actuated Manipulators 315

hinges containing a mix of passive and active component degrees of freedom hasbeen replaced by an equivalent manipulator model containing only active and pas-sive hinges.

The number of passive hinges in the manipulator is denoted np, and Ip denotesthe set of their indices. Ia denotes the corresponding set of indices of the activehinges, and na = (n−np) represents the number of active hinges in the manipula-tor. The total number of passive degrees of freedom is denoted Np, while the totalnumber of active degrees of freedom is denoted Na. Observe that Na +Np = N,the total degrees of freedom in the system.

16.1.1 Decomposition into Passive and Active Systems

We use the Ia and Ip sets of hinge indices to decompose the manipulator into apair of manipulator subsystems: the active arm Aa, and the passive arm Ap. Aa

is the Na degree of freedom manipulator resulting from freezing all the passivehinges (i.e., all hinges whose index is in Ip), while Ap is the Np degree of freedommanipulator resulting from freezing all the active hinges (i.e., all hinges whose indexis in the set Ia). This decomposition is illustrated in Fig. 16.1.

passive hinges

ActivearmAa

PassivearmAp

Under-actuated manipulator

active hinges

Fig. 16.1 Decomposition of an under-actuated manipulator into component active andpassive arms


Let θa ∈ RNa , Ta ∈ RNa and H∗a ∈ R6n×Na denote the vector of generalized

velocities, the vector of generalized forces and the joint map matrix for arm Aa.Similarly, let θp ∈RNp , Tp ∈RNp and H∗

p ∈ R6n×Np denote the corresponding

quantities for arm Ap. The two vectors θa ∈RNa and θp ∈RNp , also represent adecomposition of the vector of generalized velocities θ in a manner consistent withthe Ia and Ip sets, respectively. Similarly Ta and Tp are decompositions of T, andH∗

a and H∗p are decompositions of H∗. The columns of H∗ that correspond to the

active hinges appear as columns of H∗a, while those that correspond to the passive

hinges appear as columns of H∗p. Thus, the stacked vector of relative hinge spatial

velocities can be expressed as follows:

ΔV3.7= H∗ θ=H∗

a θa +H∗pθp (16.1)

16.1.2 Partitioned Equations of Motion

We use the arm decomposition to partition the equations of motion in (9.2) asfollows:

(

Maa Map

M∗ap Mpp

)[

θa

θp

]

+

[

Ca

Cp

]

=

[

Ta

Tp

]

(16.2)

where, with i, j ∈ {p,a}

Mij�= HiφMφ∗H∗

j and Ci�= Hiφ[b+Mφ∗a] (16.3)

In (16.2), the sub-matrices Maa and Mpp are the mass matrices for the Aa and Ap

arms, respectively.For manipulator control, we need to compute the actuator forces required to ob-

tain a desired motion of the active hinges and the resulting motion induced at thepassive hinges. That is, it is necessary to compute the Ta active hinge forces requiredto obtain a desired θa active hinge acceleration and the θp acceleration induced atthe passive hinges. We assume that the Tp passive hinge forces vector is knownfrom the (θ, θ) state of the system. These passive hinge forces typically arise fromphenomena such as friction, backlash, and stiffness.

Thus, the problem consists of evaluating the required Ta active hinge forces, andthe induced θp passive hinge accelerations for a desired θa generalized accelera-tion at the active hinges. It is assumed that (θ, θ) system state, and Tp generalizedforces at the passive hinges are known. The form of (16.2) is not convenient, sinceit contains a mix of the known and unknown quantities on both the left- and right-hand sides of the equation. A transformation of (16.2) that expresses the unknownquantities in terms of the known quantities is as follows:

[

Ta

θp

]

=

(

Saa Sap

−S∗ap Spp

)[

θa

Tp

]

+

[

Ca −SapCp

−SppCp

]

(16.4)


whereSaa

�= Maa −MapM−1

ppM∗ap

Sap�= MapM−1

pp

Spp�= M−1

pp

(16.5)

The expressions in (16.5) require Mpp to be invertible. Mpp is the mass matrixof the passive arm Ap, and it is easy to verify that since (H,φ,M) define an SKOmodel, so do the (Hp,φ,M) operators for the passive arm. Thus, Mpp is invertible.

The direct use of the expression on the right of (16.4) to obtain θp and Ta

requires the computation of M, the inversion of Mpp and the formation of variousmatrix/matrix and matrix/vector products. The computational cost of this dynamicsalgorithm is cubic in Np and quadratic in Na. Later, in Sect. 16.2, we describe asimpler O(N) algorithm whose computational complexity is only linear in bothNa and Np.

16.1.3 Spatial Operator Expression for M−1pp

Since the Mpp mass matrix of the Ap passive arm is from an SKO model, theSKO operator factorization and inversion techniques can be used to obtain aclosed form spatial operator expression for M−1

pp. Thus, following the develop-ment in Sect. 9.5 on page 179, we define the articulated body inertia quantities,P(.),Dp(.),Gp(.),Kp(.),τp(.),P+(.) andψ(., .) for the Ap arm. using the recursiveAlgorithm 16.1.

Algorithm 16.1 Articulated body inertias for the Ap passive arm⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(0) = 0

for k = 1 · · ·nP(k) = φ(k,k− 1)P+(k− 1)φ∗(k,k− 1)+M(k)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

if k /∈ IpP+(k) = P(k)

else

Dp(k) = Hp(k)P(k)H∗p(k)

Gp(k) = P(k)H∗p(k)D−1

p (k)

Kpτp(k) = I−Gp(k)Hp(k)

P+(k) = τp(k)P(k)

end if

end loop

(16.6)


Algorithm 16.1 differs from the standard articulated body Riccati equations solutionin (9.33). The difference is that the articulated body inertia quantities are being com-puted for only the Ap passive arm, and not the full arm. The algorithm skips thearticulated body inertia steps for the active bodies, since these hinges are frozenin the Ap arm. The recursion in (16.6) proceeds from the tip to the base of themanipulator. At each hinge, the active or passive status of the hinge is checked.Depending on the status of the hinge, the appropriate computations are carried outand the recursion proceeds to the next hinge. This continues until the base-body isreached.

Interestingly, the P(k) steps for the active hinges in (16.6) are exactly the recur-sion steps for computing composite body inertias seen in (9.20) on page 169. This isbecause, the freezing of the active hinges is equivalent to converting the neighboringbodies into a composite body. In the extreme case, when all hinges in the system areactive hinges, the resulting P(k) at each hinge is precisely the R(k) composite bodyinertia at the hinge. Conversely, when all the hinges are passive, the P(k) at eachhinge is precisely the articulated body inertia of the system at the hinge. In general,though, P(k) for the Ap arm is obtained by a hybrid combination of the compositeand articulated body inertia steps for the system.

Using the development in Sect. 9.5, the operator P ∈ R6n×6n is defined as ablock diagonal matrix with its kth diagonal element being P(k) ∈R6×6. The quan-tities in (16.6) are also used to define the following spatial operators:

Dp�= HpPH∗

p ∈RNp×Np

Gp�= PH∗

pD−1p ∈R6n×Np

Kp�= EφGp ∈R6n×Np

τp�= I−GpHp ∈R6n×6n

Eψ�= Eφτp ∈R6n×6n

(16.7)

The Dp,Gp and τp operators are all block diagonal. Even though Kp and Eψ are notblock diagonal matrices, their only non-zero block elements are the Kp(k,k− 1)and ψ(k,k− 1) elements, respectively along the first sub-diagonal. Following(9.36), P satisfies the Riccati equation

M = P−EψPE∗ψ = P−EφPE∗

ψ (16.8)

From Lemma 9.11, it follows that the Innovations Factorization of the mass matrixMpp is given by

Mpp = [I+HpφKp]Dp[I+HpφKp]∗ (16.9)


Also, the closed form operator expressions for the inverse of the [I+HpφKp] factorand M−1

pp are

[I+HpφKp]−1 = [I−HpψKp]

M−1pp = [I−HpψKp]∗D−1

p [I−HpψKp](16.10)

16.1.4 Operator Expressions for S Blocks

Lemma 16.1 below uses the operator expression for the inverse of Mpp to derivenew closed form operator expressions for the Sij matrices from (16.5),

Lemma 16.1 Operator expressions for S components.

Spp = [I−HpψKp]∗D−1p [I−HpψKp] (16.11a)

Sap = Ha

{ψKp +Pψ∗H∗

pD−1p [I−HpψKp]

}= Ha

{(ψ−PΩ)Kp +Pψ∗H∗

pD−1p

}(16.11b)

Saa = Ha

[

ψMψ∗ −PΩP]

H∗a

= Ha

[

(ψ−PΩ)P+Pψ∗]H∗a (16.11c)

whereΩ

�= ψ∗H∗

pD−1p Hpψ (16.12)

Ω defined above is the extended operational space compliance matrix for the Ap

passive arm as defined in (10.12) on page 190.Proof: Equation (16.11a) follows directly from (16.5) and (16.10).

With regards (16.11b):

Sap16.5= MapM−1

pp

16.3,16.11a= HaφMφ∗H∗

p[I−HpψKp]∗D−1p [I−HpψKp]

9.45= HaφMψ∗H∗


9.41= Ha

{φKp +Pψ∗H∗

pD−1p

}[I−HpψKp]

9.45= Ha

{ψKp +Pψ∗H∗


}16.12= Ha


pD−1p

}

For (16.11c),


Saa16.5= Maa −MapM−1

ppM∗ap

16.5= Maa −SapM∗

ap

16.11b= Ha

{φ−ψKpHpφ−Pψ∗H∗D−1

p ∗

[I−HpψKp]Hpφ}Mφ∗H∗

a

9.43,9.45= Ha

{φ−(φ−ψ)−Pψ∗H∗D−1

p Hpψ}

Mφ∗H∗a

=Ha

{ψ−Pψ∗H∗D−1

p Hpψ}

Mφ∗H∗a

16.12= Ha {ψ−PΩ}Mφ∗H∗

a10.38c= Ha

{(ψ−PΩ)P+Pψ∗}H∗

a

The expressions for the Sij matrices in Lemma 16.1 require only the inverse ofthe block-diagonal matrix Dp, an inverse that is relatively easy to obtain.

Exercise 16.1 Alternate expression for Saa.Show that an explicit operator expression for Saa is as follows:

Saa =Haφ(U∗⊥MU⊥)φ∗H∗

a (16.13)

where the U =ΩM and U⊥ = I−U are the projection operators defined in (10.34).

Recall that the mass matrix of the full system is HφMφ∗H∗, while that of theactive arm Aa is HaφMφ∗H∗

a. Thus, from (16.3), the mass matrix of the under-actuated manipulator, Saa, differs from the mass matrix of the active manipulator,Maa, in the presence of the U∗

⊥MU⊥ term instead of M. However, unlike M,U∗⊥MU⊥ is not block-diagonal.

16.2 O(N) Generalized Dynamics Algorithms

One of the primary computations for manipulator control is the computation of theactuator generalized forces Ta needed to obtain a desired acceleration θa at theactive hinges. A secondary need is the computation of the resulting θp accelerationinduced at the passive hinges. We derive expressions for θp, the active hinge forcesvector Ta, and the link spatial accelerations vector α in Lemma 16.2 below.

Lemma 16.2 Operator expressions for θp and Ta.With

a�= H∗

a θa +a

z�= ψ [KpTp +b+Pa]

εp�= Tp −Hpz

νp�= D−1

p εp

(16.14)

16.2 O(N) Generalized Dynamics Algorithms 321

we have

α = ψ∗ [

H∗pνp +a

]

(16.15a)

θp = [I−HpψKp]∗νp −K∗pψ

∗a = νp −Kp∗α (16.15b)

Ta = Ha

{z+P[ψ∗H∗

pνp + ψ∗a]}

=HaP [α−a+z] (16.15c)

Proof: From (16.4) we have that

θp = Spp[Tp −Cp]−S∗apθa

9.45,16.11b= [I−HpψKp]∗D−1

p {Tp −Hpψ(KpTp +b+Mφ∗a)}

−{Kp

∗ψ∗ +[I−HpψKp]∗D−1p HpψP

}H∗

a θa

9.45,9.41= [I−HpψKp]∗D−1

p {Tp −Hpψ(KpTp +Pa+b)}

−K∗pψ

∗a−{Kp

∗ψ∗ +[I−HpψKp]∗D−1p HpψP

}H∗

a θa

16.4= [I−HpψKp]∗D−1

p {Tp −Hpψ(KpTp +Pa+b)}−K∗pψ

∗a16.14= [I−HpψKp]νp −K∗

pψ∗a

This establishes the first equality in (16.15b). Also,

α9.1= φ∗[H∗ θ+a]

16.1,16.4= φ∗[H∗

p θp +a]

16.15b= φ∗H∗

p

{[I−HpψKp]∗νp −K∗

pψ∗a

}+φ∗a 9.43,9.45

= ψ∗[H∗pνp +a]

This establishes (16.15a).Using (16.15a) in the first equality in (16.15b) establishes its second equality.

Now

SapCp16.3,16.11b

= Ha

{ψKp +Pψ∗H∗


}Hpφ[b+Mφ∗a]

16.15b,9.45= Ha

{(φ−ψ)+Pψ∗H∗

pD−1p Hpψ

}[b+Mφ∗a]

12.2,16.12= Ca −Ha(ψ−PΩ)[Mφ∗a+b]

= Ca −Ha(ψ−PΩ)Mφ∗a−Ha(ψ−PΩ)b

9.42,10.38a= Ca −Ha(ψP+Pφ∗−P(φ∗ −ψ∗+ΩP))a−Ha(ψ−PΩ)b

= Ca −Ha(ψ−PΩ)[Pa+b]−HaPψ∗a(16.16)

Thus, we have that


Ta16.4= Saa θa +Sap[Tp −Cp]+Ca

16.11c,16.11b,16.16= Ha

{(ψ−PΩ)P+Pψ∗}H∗

a θa

+Ha


aD−1p

}Tp

+Ha(ψ−PΩ)[Pa+b]+HaPψ∗a

=Ha

[

(ψ−PΩ)[

PH∗a θa +KpTp +(Pa+b)

]

+P{ψ∗H∗

a θa +ψ∗H∗aD−1

p Tp + ψ∗a}]

16.14,16.12,16.4= Ha

[[

I−Pψ∗H∗pD−1

p Hp

]

z+P{ψ∗a+ψ∗H∗

pD−1p Tp

}]

=Ha

[

z+Pψ∗H∗pD−1

p (Tp −Hpz)+Pψ∗a]

16.14= Ha

[

z+Pψ∗H∗pD−1

p ε+Pψ∗a]

16.14= Ha

[

z+P(

ψ∗H∗pν+ ψ∗a

)] 16.15a= Ha [z+P(α−a)]

This establishes (16.15c).Algorithm 16.2 provides a recursive O(N) procedure for computing the θp and

Ta vectors. It is a conversion of the closed form operator expressions of the vectorsin Lemma 16.2, into a recursive tip-to-base sweep followed by a base-to-tip sweep.

The recursion in (16.17a) starts from the tip of the manipulator and proceedstowards the base. At each hinge, the active/passive status of the hinge is checked. Ifthe hinge is active, its acceleration is known, and is used to update the z(·) residualforce. On the other hand, if the hinge is passive, its generalized force is known, andis used to update the z(·) residual force. The recursion continues until the base isreached. Now begins the recursion in (16.17b) from the manipulator base towardsthe tip. This time, as each new hinge is encountered, its θp hinge acceleration iscomputed if it is a passive hinge; the unknown Ta generalized force is computed ifit is an active hinge. This continues until the tip is reached and all the hinges havebeen processed. In summary, this dynamics algorithm requires the following threesteps:

1. The recursive computation of all the V(k) link velocities and the a(k) and b(k)Coriolis terms using a base-to-tip recursion sweep, as in the standard Newton–Euler inverse dynamics Algorithm 9.1 on page 166.

2. Recursive computation of the articulated body quantities using the tip-to-baserecursive sweep described in Algorithm 16.1.

3. The inward tip-to-base recursive sweep in (16.17a) to compute the residual forcesz(k). This is followed by the base-to-tip recursive sweep in (16.17b) to computethe components of θp, Ta and α.

The recursions in Step (2) can be combined and carried out in conjunction with thetip-to-base sweep in Step (3).

An interesting feature of this algorithm is that its structure is a hybrid of knowninverse and forward dynamics algorithms for fully-actuated manipulators. When allthe hinges are passive, Ia is empty and the steps in the above algorithm reduce to

16.2 O(N) Generalized Dynamics Algorithms 323

Algorithm 16.2 Generalized AB dynamics for under-actuated systems

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

z(0) = 0

for k = 1 · · ·n⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

if k ∈ Iaz(k) = φ(k,k− 1)z+(k− 1)+b(k)+

P(k)[H∗(k)θa(k)+a(k)]

z+(k) = z(k)

else

z(k) = φ(k,k− 1)z+(k− 1)+b(k)+P(k)a(k)

εp(k) = Tp(k)−H(k)z(k)

z+(k) = z(k)+Gp(k)εp(k)

νp(k) = D−1p εp(k)

end if

end loop

(16.17a)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

α+(n+ 1) = 0

for k = n · · ·1α+(k) = φ∗(k+ 1,k)α(k+ 1)⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

if k ∈ Iaf(k) = P(k)α+(k)+ z(k)

Ta(k) = H(k)f(k)

else

θp(k) = νp(k)−G∗p(k)α+(k)

end if

α(k) = α+(k)+H∗(k)θ(k)+a(k)

end loop

(16.17b)

the AB forward dynamics algorithm. In this case, P(k) is the articulated body in-ertia of all the links outboard of the kth link. In the other extreme case, when allthe hinges are active, Ip is empty, and the steps in the algorithm reduce to the com-posite rigid body inertias based inverse dynamics algorithm discussed in Exercise5.5 on page 91. This algorithm can thus, be regarded as a generalized dynamicsalgorithm which can be used for both the inverse and forward dynamics problemsfor manipulators.

For a general under-actuated manipulator with both passive and active hinges,P(k) is formed by a combination of articulated and composite body inertia type


computations for the links outboard of the kth hinge. It is in fact the articulated bodyinertia for all the links outboard of the kth link for the passive arm Ap. Similarly,z(k) is a hybrid mix of the residual forces in the AB forward dynamics and thecomposite rigid body inertias based inverse dynamics algorithms.

Since each recursive step in the above algorithm has a fixed computational costper degree of freedom, the overall computational cost of the algorithm is linear inboth Na and Np, i.e., it is linear in N. The computational cost per passive degree offreedom is larger than the corresponding cost for an active degree of freedom. Non-zero generalized forces at the passive hinges are accounted for in a very naturalmanner in the algorithm. Also, the overhead associated with transitions betweenpassive and active status of the hinge is small. When such a transition occurs duringrun-time, the only change required is to update the Ip and Ia sets.

16.2.1 Application to Prescribed Motion Dynamics

One application of the generalized Algorithm 16.2 dynamics algorithm is to han-dle prescribed motion hinges for the forward dynamics of fully-actuated systems.While the normal forward dynamics problem consists of computing the unknowngeneralized accelerations of the system given the generalized forces on the system,there are times when some of the generalized accelerations are “prescribed” andknown. In reality the problem is to compute the remaining generalized accelera-tions, and the unknown generalized forces for the prescribed degrees of freedom.Thus, the overall number of unknown and known variables does not change. Whatdoes change is that both sets contain a mix of generalized accelerations and forces.

Algorithm 16.2 is the natural generalization of the traditional AB forward dy-namics algorithm for handling such prescribed motion hinges. The adaptation asso-ciates prescribed motion hinges with the active Ia bodies, and non-prescribed hingeswith the passive Ip bodies [51, 82]. This technique is also referred to as hybrid dy-namics in Featherstone [48].

16.3 Jacobians for Under-Actuated Systems

The end-effector Jacobian matrix, J, is widely used in motion planning and con-trol of fully-actuated manipulators. For such manipulators, J describes the velocitydomain relationship between the incremental motion of the controlled (i.e., all thehinge) degrees of freedom and the incremental motion of the end-effector frame asfollows:

Vnd3.53= Jθ = B∗φ∗H∗ θ (16.18)

16.3 Jacobians for Under-Actuated Systems 325

Vnd denotes the spatial velocity of the end-effector. B denotes the pick-off operatorfor the end-effector node. The J Jacobian is independent of dynamical quantitiessuch as link masses and inertias, and depends only upon their kinematical properties.

For under-actuated systems, control requires not only the control of the end-effector motion but also of the coupled motion induced in the passive joints. Otherthan in exceptional cases, relationships such as (16.18) cannot be expressed at thevelocity-level, but instead must be expressed at the acceleration-level as follows:

αnd = JG θa + non-acceleration dependent terms (16.19)

andθp = JD θa + non-acceleration dependent terms (16.20)

The non-acceleration dependent terms on the right-hand sides of (16.19) and (16.20)depend on the manipulator state and the passive hinge forces. The coefficient ma-trices JG and JD in (16.19) and (16.20) characterize (in the acceleration domain)the effect of the incremental motion of the controlled active hinges on the incre-mental motion of the end-effector and the passive hinges, respectively. Unlike thefully-actuated Jacobian J, the JG and JD Jacobians define acceleration level, andnot velocity level, mappings. However, they do define the relationship between theincremental motions of the controlled hinges and the quantities being controlled.We refer to JG as the generalized Jacobian and JD as the disturbance Jacobian.JD describes the “disturbance” motion induced in the passive degrees of freedomby the motion of the active hinges. The disturbance Jacobian is not meaningful forfully-actuated manipulators, since these manipulators have no passive degrees offreedom. Like JG, the disturbance Jacobian JD also depends on both the dynamicsas well as the kinematical properties of the links.

The properties of these pairs of Jacobian matrices are important for the develop-ment of control algorithms for such manipulators. Singularity analysis of JG is usedto study the desirable and undesirable regions of the workspace.

Exercise 16.2 Expression for the generalized Jacobian.Show that the JG generalized Jacobian has the general form:

JG = Ja +JpJD (16.21)

where Ja�= Bφ∗H∗

a and Jp�= Bφ∗H∗

p are the active and passive sub-blocks,respectively, of the fully-actuated J Jacobian matrix.

16.3.1 The Generalized Jacobian JG

The generalized Jacobian JG ∈R6×na in (16.19) defines the relationship betweenthe generalized accelerations of the active hinges and the spatial acceleration of the


end-effector frame. The expression for the generalized Jacobian JG is derived in thefollowing lemma.

Lemma 16.3 The generalized Jacobian JG.The generalized Jacobian JG is given by

JG = B∗[ψ∗ −ΩP]H∗a = B∗U⊥φ∗H∗

a (16.22)

Proof: Combining together the expressions in (16.15), it follows that the expres-sion for the link spatial accelerations α is

α16.18= [ψ∗ −ΩP]H∗

aθa +ψ∗H∗pD−1[I−HpψKp]Tp −Ω[b+Pa]+ψ∗a

Thus, the spatial acceleration of the end-effector frame, αnd is given by

αnd = B∗α+ B∗Vnd

= B∗[ψ∗ −ΩP]H∗aθa +B∗ψ∗H∗

pD−1[I−HpψKp]Tp

−B∗Ω[b+Pa]+B∗ψ∗a+ B∗Vnd

The first equality in (16.22) follows from comparing this with (16.19), and the sec-ond from the further use of (10.39a).

Comparing (16.22) with (16.18), we see that the JG differs from the Ja fully-actuated Jacobian sub-block in the additional presence of the U⊥ projection operatorterm. Non-kinematical mass and inertia properties of the links enter into the JG

generalized Jacobian U⊥. The computation of JG can be carried out recursively, asdescribed in Algorithm 16.3. The computational cost of this algorithm is O(Nna).

On the other hand, to compute the JGx product for a given na dimensionalvector x, Step (3) of the algorithm should be changed to set θa = x. Then a singleevaluation of Steps (4) and (5) will result in JGx.

16.3.2 Computed-Torque for Under-Actuated Systems

The generalized Jacobian can be used to compute the feed-forward active hinge gen-eralized forces Ta(t) time profile required to achieve a desired end-effector time tra-jectory. The end-effector trajectory is defined by the time profile of the end-effectorspatial acceleration αnd over the time interval of interest. We assume that the stateof the manipulator is known at the beginning, i.e., the configuration θ(t0) and hingevelocities θ(t0) are known at the initial time t = t0. A brief sketch of the computa-tional steps at time t is described in Algorithm 16.4.

This iterative procedure results in a time profile for the actuator forces Ta(t) re-quired to achieve the desired end-effector trajectory. It also computes the trajectoryof the passive hinges for the whole time interval. For simplicity, we have assumed

16.3 Jacobians for Under-Actuated Systems 327

Algorithm 16.3 Computation of the generalized Jacobian, JG

The following procedure leads to the computation of the JG generalized Jacobian:

1. First, all the hinge velocities are set to zero. This makes the nonlinear velocitydependent terms, a(k) and b(k), zero for all the links. Also, the passive hingeforces, Tp, are set to zero.

2. Next, all the articulated body quantities are computed using the tip-to-base re-cursion in Algorithm 16.1.

3. Now, set the hinge accelerations as follows:

θa(i) =

{1 for i = k

0 for i �= k

Use the tip-to-base and base-to-tip recursions in (16.17) to compute the αnd

end-effector spatial accelerations. αnd is the kth column of JG.

Repeating Step 3 for each of the na columns yields the complete generalized Jaco-bian matrix JG.

Algorithm 16.4 Computed torque for an under-actuated system

1. Compute JG(t) using Algorithm16.3.2. Assuming that JG is invertible, use (16.19) to compute θa(t) via

θa(t) = J−1G (t)

[

αnd(t)− velocity and Tp dependent terms]

3. For this θa(t), compute Ta(t) and θp(t) using the generalized dynamics algo-rithm in Sect. 16.2.

4. Integrate the hinge accelerations (or use measured values) to obtain the hingevelocities θ(t+Δt) and configurations θ(t+Δt) at time t+Δt. Go back to Step1 and repeat the steps for time (t+Δt).

above that JG is square and non-singular. When JG is non-invertible, due to be-ing singular or non-square, this procedure can be modified to use methods suchas pseudo-inverses and least-squares solutions in ways similar to those for fully-actuated redundant manipulators.

16.3.3 The Disturbance Jacobian JD

The disturbance Jacobian JD in (16.20) characterizes the inertial coupling betweenthe active and the passive hinges. It describes the incremental disturbance motioninduced in the passive hinges due to the incremental motion of the active hinges.The following lemma provides an operator expression for JD.


Lemma 16.4 The disturbance Jacobian JD.The operator expression for the disturbance Jacobian JD is given by:

JD = −S∗ap

= −[I−HpψKp]∗D−1p HpψPH∗

a −Kp∗ψ∗H∗

a

(16.23)

Proof: From (16.4), the passive hinge accelerations are given by

θp = −S∗ap θa +Spp[Tp −Cp] (16.24)

and the result follows.Using Algorithm 16.3, the computation of JD can be carried out simultaneously

with the computation of JG. The kth column of JD is simply the θp vector ob-tained during the steps for the computation of the kth column of JG. The computa-tional cost of this algorithm is also O(Nna).

In applications where a larger number of active degrees of freedom are availablethan are needed to meet the primary objective of end-effector motion control, theredundant active degrees of freedom can be used to meet secondary objectives suchas controlling the passive hinge motion to minimize disturbances. In this scenario,at each control sample time, the following steps are executed:

1. Algorithm 16.3 is used to recursively compute the generalized and disturbance

Jacobians JG and JD, and to form the composite Jacobian quantity

[

JG

JD

]

.

2. The combination of (16.19) and (16.24) characterizes the effect of the activehinge accelerations θa upon the end-effector acceleration αnd and the passivehinge accelerations θp as follows:

[

JG

JD

]

θa =

[

αnd

θp

]

+ velocity and Tp dependent terms (16.25)

Equation (16.25) is solved for the active hinge acceleration θp using the desiredvalue for the end-effector acceleration and a value of zero for the acceleration atthe passive hinges. The composite Jacobian matrix might not be square in mostcases. When there are only a limited number of degrees of freedom, (16.25) canonly be solved approximately, and some performance will be lost. On the otherhand, when there are sufficient redundant degrees of freedom, a solution can bechosen to meet additional performance objectives.

3. Next, the active hinge accelerations are used in the generalized dynamics Algo-rithm 16.2 to compute the active hinge forces Ta.

16.4 Free-Flying Systems as Under-Actuated Systems 329

16.4 Free-Flying Systems as Under-Actuated Systems

Free-flying systems (e.g. space platforms) with inactive base-body control are animportant special case of under-actuated systems. We look at some of their proper-ties and discuss the application of the under-actuated formulation and algorithms tothese systems. The configuration considered consists of a manipulator mounted on afree-flying space vehicle. The space vehicle is controlled in six degrees of freedomby an attitude and translation control system. The manipulator motion is controlledby actuators acting at the hinges of the manipulator.

Performing manipulation maneuvers with the attitude and translation control sys-tem inactive most of the time can help conserve fuel, and is referred to as reaction-mode control. The control system turns on when the disturbance motions in thebase-body exceed prescribed bounds. One of the desirable goals of space manipula-tor control is to plan and execute manipulator motions that minimize the activationof the control system to conserve fuel. The internal hinges of the space manipu-lator represent the active degrees of freedom, while the six base-body degrees offreedom represent passive degrees of freedom. During reaction-mode control, thepassive hinge forces are zero, i.e., Tp = 0. These forces are non-zero only when thespace vehicle control system is on. The motion planning problem for space manip-ulators consists of computing active hinge forces to execute a desired end-effectortrajectory while minimizing base-body motion. If the manipulator has redundant de-grees of freedom, then these can be used to minimize the motion of the base-body[4, 125, 130, 134, 177].

Unlike general under-actuated systems, the linear and angular momentum offree-flying systems possess conservation properties. Also, unlike under-actuatedsystems, but like fully-actuated systems, the generalized and disturbance Jacobianscan define velocity-level maps instead of acceleration-level maps. Due to these spe-cial properties, the behavior free-flying system falls between that of fully actuatedand general under-actuated systems.

16.4.1 Integrals of Motion for Free-Flying Systems

We describe here an informal derivation of the conservation property of the spatialmomentum of free-flying systems. Such conservation properties are referred to asintegrals of motion. Noether’s theorem from analytical mechanics [11] shows thatsuch invariances arise from symmetries in the kinetic energy Lagrangian of the sys-tem. An example of such an integral of motion is the conservation of the systemkinetic energy in the absence of external forces. This integral of motion arises be-cause the expression for the kinetic energy does not explicitly depend on time.

A special case of such a symmetry arises when the kinetic energy does not de-pend upon some of the generalized coordinates of the system. Such generalizedcoordinates are referred to as ignorable coordinates [153], and lead to additionalintegrals of motion. For free-flying systems, the six base-body degrees of freedom


are ignorable coordinates, because the kinetic energy of the system does not ex-plicitly depend on the orientation, or location, of the system in free-space. With themanipulator kinetic energy given by 1

2β∗Mβ, the subset of the Lagrangian equa-

tions of motion from (4.24) corresponding to the passive degrees of freedom, thelower half of (16.2), can be expressed as:

d[M∗apβa +Mppβp]

dt−

12∇θp [β∗Mβ] = Tp (16.26)

Since M, and consequently the kinetic energy, does not depend on the base-bodyhinge generalized coordinates θp, it follows that

∇θp [β∗Mβ] = 0

The left-hand side of (16.26) is, therefore, an exact differential, and can be rewrittenin the form

M∗apβa +Mppβp = [M∗

ap, Mpp]β =

∫t

t0

Tpdt+ constant (16.27)

The left-hand side of (16.27) is precisely the system-level 6-vector spatial momen-tum of the whole space manipulator at time t. The constant on the right-hand side isthe spatial momentum at time t0, and the integral term reflects the rate of change ofthe momentum. During reaction-mode control, Tp = 0, and therefore the left-handside of (16.27) is constant, i.e., the linear and angular momentum of the manipulatorare conserved, and are integrals of motion for the manipulator.

Equation (16.27) is equivalent to a time-varying constraint on the generalizedvelocities of the system. The conservation of linear momentum is a holonomic con-straint and implies that the center of mass of the manipulator remains stationary. Onthe other hand, the conservation of angular momentum represents a non-holonomicconstraint. Methods using these constraints have been developed for analyzing thekinematics, dynamics and control of space manipulators [44, 129, 134, 176]. Thesemethods have primarily focused on the case when the right side of (16.27) is zero,i.e., when the manipulator has zero spatial momentum and is undergoing reaction-mode control. The following exercise derives a simple form of the disturbance Ja-cobian for free-flying systems.

Exercise 16.3 The disturbance Jacobian for a free-flying system.Show that the disturbance Jacobian for the free-flying system is given by:

JD = −R−1(n)[

φ(n,1)R(1)H∗(1), · · · ,

φ(n,n− 1)R(n− 1)H∗(n− 1)] (16.28)

where R(k) denotes the composite body inertia at the kth link.

Chapter 17Free-Flying Systems

In addition to the internal hinge degrees of freedom, free-flying manipulators have 6additional degrees of freedom associated with the overall location and orientation ofthe manipulator. These additional 6 degrees of freedom are normally assigned to themanipulator base-body. While the base-body link is typically chosen based on de-sign and use considerations, from a more general perspective, any one of the manip-ulator links can be designated as the base-body with equal validity. This freedom ofchoice, not available for ground manipulators, is responsible for a base-invariancesymmetry possessed by free-flying manipulators [83, 148]. In this chapter, we usethis symmetry to study free-flying manipulator dynamics.

17.1 Dynamics of Free-Flying Manipulators

While the only fundamental requirement is that the free-flying system have an SKOmodel, for the sake of exposition, we initially focus on an n rigid-link, free-flyingcanonical serial-chain manipulator. Once again, there is no loss in generality sincethe ideas developed here easily extend to general SKO models using the approachfrom Chap. 9.

17.1.1 Dynamics with Link n as Base-Body

For free-flying manipulators, the nth hinge (between the base-body and the inertialframe) has 6 degrees of freedom, and the components of the generalized velocitiesvector,β(n), for this hinge are the components of the 6-dimensional spatial velocityof the base-body, V(n). The H(n) hinge map matrix for the nth hinge is the 6×6identity matrix, i.e.,H(n) = I6. This model, with the nth link as the base body link,is illustrated in Fig. 17.1, and is referred to as the regular model. The SKO oper-ator factorization and inversion result, as well as the O(N) AB forward dynamics


332 17 Free-Flying Systems

1

k

n6 dof hinge

Basebody

Inertial Reference

Fig. 17.1 A serial free-flying manipulator with a 6 degree of freedom hinge at the base-body

algorithm for regular manipulators, extend to free-flying manipulators as well. SinceH(n) = I6, the nth recursion steps (for the base-body) in (9.33) on page 175 for-ward dynamics recursion simplify to

D(n) = P(n), G(n) = I6, τ(n) = I6, τ(n) = 06 (17.1)

The residual force computations in Algorithm 9.5 also simplify to

ε(n) = T(n)− z(n), θ(n) = ν(n) = P−1(n)ε(n), α(n) = ν(n)+a(n) (17.2)

For a ground manipulator, the choice of the base-body is most conveniently the linkattached to the ground. For free-flying manipulators, since none of the links areattached to the ground, the choice of the base-body link is in principle an arbitrarychoice.

17.1.2 Dynamics with Link 1 as Base-Body

Now, we examine an alternate model for the serial-chain free-flying manipulator,where link 1 is designated the base-body for the manipulator. We refer to this newmodel as the dual model.

We use the subscript “dl” for quantities associated with the dual model (with link1 as base-body) to distinguish them from those associated with the regular model.Thus, the vectors β and βdl denote the generalized velocity vectors for the regularand dual manipulator models, respectively.

17.1 Dynamics of Free-Flying Manipulators 333

Figure 17.2 shows the configuration of a serial free-flying manipulator with link1 chosen as the base-body. As discussed in Sect. 17.3, the six components of the

1

k

n

6 dof hinge

Basebody

Inertial Reference

Fig. 17.2 A serial free-flying manipulator with the outer most link as base-body

spatial velocity vector of the base-body form part of the generalized velocity co-ordinates for the manipulator. Thus, when the base-body is moved from link n tolink 1, the components of β(n) in β are replaced by the components of V(1) to ob-tain the new generalized velocity coordinates vector βdl. The definition of theHdl,φdl etc. operators and the mass matrix Mdl all change in the dual formulation. Thereversal in direction also reverses the sense of orientation of the internal hinge axes,and, therefore all of the H(.) hinge map matrices reverse sign in the dual model.

Nevertheless, the SKO formulation operator factorization and inversion resultsfor the new mass matrix still apply. Consequently, there is a corresponding versionof the AB forward dynamics algorithm (Algorithm 9.5) for this model as well. Oneimportant difference between the dual AB algorithm and Algorithm 9.5 is that thesince the roles of the base and tip links are reversed, the tip-to-base (base-to-tip)recursions now proceed from link n to 1 (link 1 to n).

Exercise 17.1 Weight matrices for the dual model.Assuming that the φ(k,k− 1) weight matrices of the regular model SKO operatorare invertible, show that the SKO weight matrix elements for the dual model, denotedφ(k− 1,k), are the inverses of φ(k,k− 1), i.e.,

φ(k− 1,k)�= φ−1

(k,k− 1) =

(

I3 −˜l(k,k− 1)

0 I3

)

(17.3)

We designate the dual articulated body inertia by the symbol S+(k), and add thesubscript “dl” to the other quantities, e.g., Ddl, Gdl, τdl. The quantity correspond-ing to P+ in the dual formulation is designated S, and is given by the expression


S�= τdlS

+

The articulated body inertia recursions for the S(k+ 1) dual articulated body inertiasand related quantities are described in Algorithm 17.1. The correspondence betweenthe regular and dual articulated body inertia quantities is illustrated in Fig. 17.3.Observe that the indices for P(k) and S(k) run from k = 1 · · ·n, while those forP+(k) and S+(k) run from k = 0 · · ·n− 1.

Algorithm 17.1 Computation of dual articulated body inertias

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

S(n) = 0

for k = n− 1, · · ·0S+

(k) = φ(k,k+ 1) [S(k+ 1)+M(k+ 1)]φ∗(k,k+ 1)

Ddl(k) =H(k)S+(k)H∗

(k)

Gdl(k) = −S+(k)H∗

(k)D−1dl (k)

Kdl(k− 1,k) = φ(k− 1,k)Gdl(k)

τdl(k) = −Gdl(k)H(k)

τdl(k) = I6 −τdl(k)

S(k) = τdl(k)S+

(k)

ψdl(k− 1,k) = φ(k− 1,k)τdl(k)

end loop

(17.4)


Ok

Ok+1

O+k

O+k−1

P(k) = φ(k+1,k)P+(k+1)φ∗(k+1,k)+M(k)S(k) = τdlS

+(k)

P+(k) = τ(k)P(k)S+(k) = φ(k,k+1)[S(k+1)

+M(k+1)]φ∗(k,k+1)P(k+1)S(k+1)

Fig. 17.3 The correspondence between the regular and dual model P(.), S(.) andP+(.), S+(.) quantities at the kth and (k+1)th links

17.1 Dynamics of Free-Flying Manipulators 335

Exercise 17.2 Dual articulated body inertia properties.

1. Analogous to the P+(k)H∗(k) = 0 identity, show that

S(k)H∗(k) = 0 (17.5)

2. Show that

τdl(k)τ(k) = 0 and τ(k)τdl(k) = 0 (17.6)

Algorithm 17.2 is the corresponding version of the residual-forces and accelera-tions sweeps in the dual AB forward dynamics algorithm. Re-expressing (5.8) usingthe dual φ(k,k− 1) weight matrices shows that the adl(k) Coriolis accelerationspatial vector in the dual model is related to that of the regular model as follows:

adl(k)�= −φ∗

(k+ 1,k)a(k+ 1) (17.7)

Algorithm 17.2 Dual AB forward dynamics algorithm

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

zdl(n) = 0

for k = n− 1, · · ·0z+dl(k) = φ(k,k+ 1)

[

zdl(k+ 1)+b(k+ 1)

+ {S(k+ 1)+M(k+ 1)}adl(k+ 1)]

εdl(k) = T(k)+H(k)zdl(k)

zdl(k) = z+dl(k)+Gdl(k)εdl(k)

νdl(k) = D−1dl εdl(k)

end loop

(17.8a)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

α(0) = 0

for k = 1 · · ·nα(k) = φ∗

(k− 1,k)α+(k− 1)

θ(k) = νdl(k)−G∗dl(k)α(k)

α+(k) = α(k)−H∗

(k)θ(k)+adl(k)

end loop

(17.8b)


17.1.3 Direct Computation of Link Spatial Acceleration

The following expression relates the inter-link forces, f(k), the residual forces z(k),and the link spatial accelerations α(k) for the regular model:

f(k)7.34= P+

(k)α+(k)+ z

+(k) = P(k)

[

α(k)−a(k)]

+z(k) (17.9)

The dual version of (17.9) is:

−f(k) = S+(k)[

α+(k)−adl(k)

]

+z+dl(k) = S(k)α(k)+ zdl(k) (17.10)

The relationships in (17.9) and (17.10) provide alternative ways of expressing theinter-link spatial force f using either the conventional or the dual articulated bodyinertia quantities. Combining these alternative expressions provides a direct methodfor computing the spatial accelerations of the links. This is described in the follow-ing lemma.

Lemma 17.1 Expression for the link spatial acceleration.The spatial acceleration, α(k), of the kth link is given by:

α(k) = −[

P(k)+S(k)]−1[

z(k)+ zdl(k)−P(k)a(k)]

k = 1 · · ·nα+

(k) = −[

P+(k)+S+

(k)]−1[

z+

(k)+ z+dl(k)−S+

(k)adl(k)]

k = 0 · · ·n− 1(17.11)

Proof: Add together (17.9) and (17.10), to eliminate f.This result implies that the α and α+ link accelerations can be obtained by com-

bining together the results from the regular and dual articulated body inertia andresidual force recursions. Once we have the link accelerations, the hinge gener-alized accelerations can be obtained by one of the following pair of expressions(see (7.32f)):

θ(k) = ν(k)−G∗(k)α+

(k) = νdl(k)−G∗dl(k)α(k) (17.12)

17.1.4 Dynamics with Link k as Base-Body

Let us now consider the intermediate link k as the base-body for the manipulator,as shown in Fig. 17.4. For this choice, the components of the V(k) spatial velocityfor the kth body provide six of the generalized velocity coordinates for the system.The manipulator now has a tree-topology configuration, with two branches startingat the base. The basic algorithm remains the same as for a serial-chain, except thatthe recursions now have a scatter/gather structure. The recursions towards the basegather the inputs from the incoming branches, while those proceeding outwards

17.2 The Base-Invariant Forward Dynamics Algorithm 337

1

k

n

6 dof hinge

Basebody

Inertial Reference

Fig. 17.4 A serial free-flying manipulator with an intermediate link as base-body

scatter their outputs along the outgoing branches. Thus, the articulated body inertiaalgorithm now involves two separate articulated body inertia recursions starting offat link 1 and link n, respectively, and proceeding independently towards link k. Thefirst recursion, from link 1 to link k, is identical to the recursion in Algorithm 9.5for computing P(.) matrices for the regular model. On the other hand, the secondrecursion from link n to link k is identical to the recursion in Algorithm 17.1 forcomputing S(.) matrices for the dual model. These recursions end when the kthbase-body is reached.

The results from the two recursions are combined at the kth link, to form thequantity P(k)+S(k). This quantity is precisely the articulated body inertia of thewhole manipulator, as seen at frame Ok, with the kth link serving as the base-body.Similarly z(k)+zdl(k) is the residual force at frame Ok with the kth link being thebase-body. The corresponding relationships in (17.11) for the “+” quantities havea similar interpretation, but with frame O

+k serving as the reference frame for the

kth link. Thus, according to Lemma 17.1, we can compute the spatial accelerationsfor the kth link by first making it the base-body, computing its articulated bodyinertia and residual forces and then using Lemma 17.1 to obtain its spatial acceler-ation. This observation forms the basis for the base-invariant forward dynamicsalgorithm described in the next section.

17.2 The Base-Invariant Forward Dynamics Algorithm

The previous section discussed how any link in a free-flying manipulator can,in principle, be regarded as a base-body. This arbitrariness in the choice of thebase-body reflects the inherent base-invariance symmetry of free-flying manipula-


tors. However, the AB forward dynamics algorithms require the designation of aspecific link as the base-body for the manipulator, and this specific choice breaksthe symmetry. Breaking this symmetry is unnecessary, and the algorithm can bereformulated to preserve, and take advantage of the symmetry. The key is to simul-taneously treat every link in the manipulator as a base-body. With this in mind,and making use of Lemma 17.1, a new forward dynamics algorithm is described inAlgorithm 17.3.

Algorithm 17.3 Base-invariant forward dynamics algorithm

1. Compute the orientations, V(k) spatial velocities and the a and b Coriolis andgyroscopic terms for all the links recursively.

2. a. Compute the P(k) etc. articulated body quantities and the z(k) residual forcesin a recursion from link 1 to link n using Algorithms 9.5.

b. Simultaneously compute the S+(k) dual articulated body quantities etc. andthe z

+dl(k) dual residual forces in a recursion from link n to link 1 using

Algorithms 17.1 and 17.2. The recursions in (a) and (b) can be carried outindependently.

3. For the kth link, compute the α(k) link spatial acceleration using Lemma 17.1,and the θ(k) hinge acceleration using (17.12). These computations can be carriedout independently for each link.

4. For each link, integrate its hinge acceleration and velocity to update its hingevelocity and angle.

17.2.1 Parallels with Smoothing Theory

As discussed in Sect. 6.4, the AB forward dynamics tip-to-base recursion is analo-gous to the recursion for generating optimal filter estimates of the state of a discrete-time dynamical system. The subsequent base-to-tip recursion, is analogous to theprocedure for updating this estimate, into the optimal smoothed estimate of the state.

An alternative formulation of the smoothing algorithm in estimation theory con-sists of running two independent Kalman filters, one causal, and the other anti-causal, to generate two sets of filtered estimates of the system state [58, 59]. Theoptimal smoothed estimate can be obtained by combining the results of this pair offilter estimates.

The structure of the base-invariant algorithm for free-flying systems, is analogousto the structure of this alternative smoothing procedure. It combines, in effect, theresults from running the first half of the AB recursion in opposing directions, anddispenses with the latter half of the AB recursion altogether.

17.2 The Base-Invariant Forward Dynamics Algorithm 339

17.2.2 Simplifications Using Non-Minimal Coordinates

So far, the hinge angles, together with the six base-body positional and orientationcoordinates, have been used as the generalized coordinates for the free-flying ma-nipulator. These coordinates form a minimal set, since their dimension is the sameas the number of degrees of freedom for the system. We now examine an alter-native and non-minimal choice of coordinates that simplifies the computations inAlgorithm 17.3.

Towards this goal, each link is treated as an independent rigid body system. Foreach link, choose its orientation and positional coordinates as its generalized co-ordinates and its spatial velocity vector as its 6-dimensional generalized velocitycoordinates. Taken together, this results in a system with 6n generalized velocitycoordinates. These coordinates are clearly non-minimal since only N of them aretruly independent. However, with these coordinates, transformations between thehinge and spatial coordinate domains become unnecessary. The modified decoupleddynamics algorithm is described in Algorithm 17.4.

The use of these non-minimal coordinates eliminates the need for the minimalθ and β coordinates and, hence, dispenses with the kinematics computations inAlgorithm 17.3 for obtaining the link spatial velocities and orientations. The pricepaid with the use of redundant coordinates is that the integration method now in-volves a differential-algebraic equation rather than an ordinary differential equa-tion, since the states being integrated are of non-minimal size. Additional stepsare needed to monitor and remove drifts during the integration of the non-minimalcoordinates.

Algorithm 17.4 Base-invariant forward dynamics algorithm with non-minimalcoordinates1. Use the generalized velocities (i.e., spatial velocity V(k)) of each link to compute

the a and b Coriolis and gyroscopic terms for the link. These can be computedindependently for each link.

2. a. Compute the P(k) etc. articulated body quantities and the z(k) residual forcesin a recursion from link 1 to link n using Algorithms 9.5.

b. Simultaneously compute the S+(k) etc. dual articulated body quantities andthe zdl(k) dual residual forces in a recursion from link n to link 1 usingAlgorithms 17.1 and 17.2. The recursions in (a) and (b) can be carried outindependently.

3. For the kth link, compute the α(k) link spatial acceleration using Lemma 17.1.4. For each link, integrate its α(k) spatial acceleration and the V(k) spatial velocity

to update its spatial velocity, position and orientation.


17.2.3 Computational Issues

As is the case for the AB forward dynamics algorithm (Algorithm 9.5), the decou-pled dynamics algorithm described in Algorithm 17.4 is also of O(N) complexity.However, since the latter involves a pair of articulated body recursions, it is com-putationally more expensive. On the other hand, since some of the computationsare decoupled and independent of each other, the algorithm has benefits for parallelimplementation. In Algorithm 17.4, the computations in step 1 can be carried outindependently and in parallel for all the links. In step 2, the articulated body recur-sion in one direction is completely independent of the one in the opposite direction.Thus, they can be computed in parallel. Using an architecture in which each linkis assigned its own computational node, each link (node) receives the results of thearticulated body recursions from its neighbors, updates its own articulated body in-ertia, and passes the results onto its neighbors. As in step 1, the computations in step3 are independent from link to link. Thus, each link computes its own spatial accel-eration independent of the other nodes. Each node even has its own local integratorto update the state of its link.

17.2.4 Extensions to Tree-Topology Manipulators

The standard AB forward dynamics algorithm for tree-topology systems has beendescribed in Algorithm 9.5. The recursive computations take on a gather/scatterstructure. Recursions proceeding from the tips towards the base gather and sum theoutputs from the incoming branches as the recursion progresses. On the other hand,the recursions that start from the base and proceed towards the tips scatter their out-puts along each of the outgoing branches. Apart from this difference, the algorithm,when extended to tree-topology systems, retains the same sequential recursion stepsas for serial-chain systems.

As in the case of serial-chain free-flying manipulators, tree-topology free-flyingmanipulators also possess the base-invariance symmetry arising from the non-unique choice for the base-body. The structure of the decoupled dynamics algorithmfor tree-topology free-flying manipulators is illustrated in Fig. 17.5, and takes intoaccount the fact there are more than two extremal bodies. The overall structure ofthe decoupled dynamics algorithm remains the same as in Algorithm 17.4. Cor-responding to each extremal body, there is a model in which the extremal body istreated as the base-body for the model. Articulated body inertia computations arerequired for each of these models. At a link with multiple branches, every branchcollects the articulated body inertia outputs from all of the other branches at thelink. This data is accumulated by each branch to allow its articulated body inertiaprocedure to proceed. The overlap in the computations among the articulated bodyinertia recursions for the models is such that there are precisely two recursions pro-ceeding in opposite directions across any serial link segment of the manipulator.Lemma 17.1 is still valid, and is used to compute the spatial accelerations of eachof the links.

17.3 SKO Model with kth Link as Base-Body 341

towardsthe “base”



away fromthe “tip”



Fig. 17.5 The structure of the decoupled dynamics algorithm for tree-topology free-flying manipulators

17.3 SKO Model with kth Link as Base-Body

We now study the transformations to the SKO and SPO operators as the base-bodyis changed from the nth link to the kth link.

17.3.1 Generalized Velocities with kth Link as the Base-Body

When link n is the base-body, its spatial velocity, V(n), defines the velocity gen-eralized coordinates for its 6 degree of freedom hinge, and is a part of the overallgeneralized velocity vector for the system as follows:

θn�=

⎡

⎢

⎢

⎣

θ(1)

...

V(n)

⎤

⎥

⎥

⎦

∈RN (17.13)

The base-body switch from the nth link to the kth link, requires that we replaceV(n) with V(k) in the system’s generalized velocity vector. The new generalized


velocity vector, with the kth link as the base-body is denoted, θk, and is defined as:

θk�=

⎡

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎣

θ(1)

...

θ(k− 1)

V(k)

θ(k)

...

θ(n− 1)

⎤

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎦

∈RN (17.14)

17.3.2 Link Velocity Recursions with kth Link as the Base-Body

The switch in base-body link requires us to re-express the link spatial velocitiesusing the new generalized velocities θk. Recall that the velocity recursions with thenth link as base-body are of the following form

V(j) = φ∗(j+ 1, j)V(j+ 1)+H∗

(j)θ(j) (17.15)

for all links. With the kth link as base-body, the spatial velocity of links on the pathfrom link n to link k have the revised form:

V(j) = φ∗(j− 1, j)V(j− 1)+φ∗

(j− 1, j)H∗(j− 1)θ(j− 1) ∀ j : n� j� k

(17.16)Equation (17.16) is a simple transformation of (17.15) such that the velocity recur-sions proceed outwards from the new base-body. Though, for simpler exposition,we continue to assume that the φ(j+ 1, j) matrices are invertible, more generallythis assumption is too strict and is not required. What is required however, is an in-verse expression of the form in (17.16), that expresses the V(j) inboard body spatialvelocity, in terms of the V(j− 1) outboard body spatial velocity and the θ(j− 1)

generalized velocity for the hinge connecting the bodies. This inverse expressiondefines the weights and component joint map matrices for the model with the newbase-body.

Observe that the velocity recursions for only the links on the path between thebase-body links n and k are affected. The expressions for all other links continueto be defined by (17.15). Thus, in effect, the impact of shifting the base-body is toreverse the direction of the path from the old to the new base-body to obtain the treegraph for the new system. This path reversal is illustrated in Fig. 17.6. It shows thatthe change in base-body affects only the links on the path joining the old and newbase-bodies. With this observation, we denote the links on the path from the old tothe new base-body as the S serial-chain sub-graph of the original tree graph. SinceS is a serial-chain, its reversed version, referred to as Sr, is a serial-chain as well,albeit a non-canonical one.


newbase-body

newbase-body

originalbase-body

originalbase-body

S sub-graphSr reversedsub-graph

Graph with original base-body Graph with new base-body

Fig. 17.6 Illustration of the S sub-graph defined by the serial-chain connecting theoriginal and the new base-body. The effect of changing the base-body is to reverse theS serial-chain sub-graph connecting the pair of bodies

17.3.3 Partitioned System

Since S is a serial-chain, it is also a path-induced sub-graph. Using Lemma 14.1, Spartitions the original graph into child, C, and parent, P path-induced forests. Sincethe linkn base-body belongs to S, P is empty, and C contains all the remaining linksin the system. Thus, using (14.2), the Eφ SKO operator and H can be expressed inpartitioned form as:

Eφ =

(

EφC 0

BS EφS

)

, H=

(

HC 0

0 HS

)

, θ =

[

θC

θS

]

(17.17)

Since the shift in the base-body only affects the S serial-chain, the SKO operator forthe transformed system is identical to that in (17.17), with the EφS sub-graph SKOoperator replaced by a new one. With this observation, we will focus for now on thetransformed properties of S, since it is the only part that changes. The generalizedvelocities of the original and reversed serial-chain S are:

θS�=

⎡

⎢

⎢

⎣

θ(k)

...

V(n)

⎤

⎥

⎥

⎦

∈RNG and θR�=

⎡

⎢

⎢

⎢

⎢

⎣

V(k)

θ(k)

...

θ(n− 1)

⎤

⎥

⎥

⎥

⎥

⎦

∈RNG (17.18)

NG denotes the length of the generalized velocities for the S serial-chain. Also,define unit vectors eGj and eRj that are dimensionally compatible with θS and θR,respectively. These unit vectors satisfy the following relationships:

e∗GjθS = θS(j) and e∗RjθR = θR(j) ∀ k� j� n (17.19)


In particular, eGn and eRk are NG×6 matrices such that

e∗Gn θS = V(n) and e∗Rk θR = V(k) (17.20)

17.3.4 Properties of a Serial-Chain SKO Operator

Assuming that S is canonical, it follows from (8.30) that

EφS =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0φ(k+ 1,k) 0 . . . 0 0

0 φ(k+ 2,k+ 1) . . . 0 0...

.... . .

......

0 0 . . . φ(n,n− 1) 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(17.21)

We can re-express EφS asEφS = SΔφ (17.22)

where

S�=

n−1∑j=k

ej+1e∗j =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 0 0 0 0

I 0 . . . 0 00 I . . . 0 0...

.... . .

......

0 0 . . . I 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

∈R6nG×6nG

and Δφ�=

⎛

⎜

⎜

⎜

⎜

⎝

φ(k+ 1,k) . . . 0 0

0. . . 0 0

0 . . . φ(n,n− 1) 0

0 . . . 0 I

⎞

⎟

⎟

⎟

⎟

⎠

R6nG×6nG

(17.23)

In the above, nG denotes the number of bodies in the S serial-chain. Δφ is block-diagonal and invertible, since the component φ(j+ 1, j) elements are invertible.Some useful properties of S are defined in the following remark.

Remark 17.1 Properties of the S matrix.Let S denote the matrix defined in (17.23). Then given compatible, block-diagonalmatrices A and B, the following relationships hold:


(SAS∗)SB= SAB

(S∗AS)S

∗B= S∗AB

AS∗(SBS

∗) =ABS

∗ (17.24)

(SAS∗)(SBS

∗) = SABS

∗

(S∗AS)(S

∗BS) = S∗ABS

(SS∗)SA= SA SA(S

∗S) = SA

(S∗S)S

∗A= S∗A A(SS

∗)S = AS

(S∗S)AS

∗= AS

∗ A(S∗S)S

∗= AS

∗(17.25)

The following additional identities hold:

AS(S∗BS) =ABS

(SS∗)AS =AS (17.26)

S∗A(SS

∗) = S

∗A

Also,

SS∗

= I− eke∗k, S∗S = I− ene∗n, e∗kS = 0, Sen = 0 (17.27)

In the above, ek denotes a block-vector with only the fist block element being non-zero (and the identity matrix), while en has only the last entry being non-zero (andthe identity matrix). These expressions can be verified directly using arbitrary block-diagonalA and B matrices.

17.3.5 Reversing the SKO Operator

The following lemma defines the SKO properties of the Sr reversed sub-graph.

Lemma 17.2 The reversed EφR operator.The SKO operator EφR, together with H∗

R and θR for the Sr reversed serial-chainare related to the original S serial-chain quantities as follows:

EφR�= Δ−1

φ S∗ (17.28a)

H∗R

�= −E∗

φRH∗

SS∗RG+ eke∗Rk (17.28b)

θR = JRG θS where JRG�= SRG+ eRke∗kφ

∗SH

∗S (17.28c)


and

SRG�=

n−1∑j=k

eRj+1 · e∗Gj ∈RNG×NG (17.29)

With φR�= (I − EφR)−1 as the SPO operator for the reversed system, the link

spatial velocities VS satisfy the following relationships:

VS = E∗φR

VS +H∗RθR and VS = φ∗

RH∗R θR (17.30)

Proof: For S, we have

VS = E∗φS

VS +H∗SθS and VS = φ∗

SH∗S θS (17.31)

Thus,

JRG θS17.28c= (SRG+ eRke∗kφ

∗SH

∗S)θS

17.31= SRG θS + eRke∗kVS

17.19,17.29=

n−1∑j=k

eRj+1θS(j)+ eRkV(k)17.18= θR

This establishes the last expression for θR in (17.28c).Furthermore,

H∗RJRG

17.28b,17.28c= (−E∗

φRH∗

SS∗RG+ eke∗Rk)(SRG+ eRke∗kφ

∗SH

∗S)

17.29= −E∗

φRH∗

SS∗RGSRG+ eke∗kφ

∗SH

∗S

17.29,17.27= −E∗

φRH∗

S + eke∗kφ∗SH

∗S

= (−E∗φR

+ eke∗kφ∗S)H∗

S

(17.32)

In the above we have used the e∗RkSRG= 0 identity (shown later in (17.37a)). Hence,

H∗R θR

17.28c= H∗

RJRG θS17.32= −E∗

φRH∗

S θS + eke∗kVS (17.33)

We have

E∗φR

VS +H∗R θR

17.33,17.31= E∗

φR(E∗φS

VS +H∗S θS)+ (−E∗

φRH∗

SθS + eke∗kV)

17.22,17.28a= SS

∗VS + eke∗kVS17.27= VS

This establishes (17.30).JRG in (17.28c) defines the transformation between the original and the new gen-

eralized velocities for the system. From (17.28b), H∗R = −SΔ−1

φ H∗SS∗RG+ eke∗Rk,

which on closer examination can be seen to be block-diagonal with the 6 × 6identity matrix as its first diagonal element. The internal structure of the EφR spatialoperator is as follows:


EφR =

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎝

0 φ(k,k+ 1) 0 0 0...

.... . .

......

0 0 . . . φ(n− 2,n− 1) 0

0 0 . . . 0 φ(n− 1,n)

0 0 . . . 0 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎠

(17.34)

Exercise 17.3 Relationship between φ∗S and φ∗

R.Show that

φ∗R+φ∗

S = φ∗Reke∗kφ

∗S (17.35)

In other words,φ∗R(φ∗

S)−1

= φ∗Reke∗k− I (17.36)

Before moving on, we list a few identities that are easy to verify.

S∗RGeRk = 0 (17.37a)

S∗RGSRG = I− ene∗n (17.37b)

φ∗Ren

8.21= en (17.37c)

e∗nH∗S = e∗n (17.37d)

e∗nφ∗S

8.20= e∗n (17.37e)

Exercise 17.4 The inverse transformation JGR.Show that

θS = JGR θR where JGR�= S

∗RG+ eGne∗nφ

∗RH

∗R

(17.38)

Also, verify thatJGR ∗JRG = I (17.39)

Thus, the JRG and JGR transformations are the inverses of each other.

17.3.6 Transformed SKO Model

With the kth link as the base-body, the new system-level SKO operator and jointmap matrix partitioned structure in (17.17) have the following form:

Eφ =

(

EφC 0

BS EφR

)

, H=

(

HC 0

0 HR

)

, θ =

[

θC

θR

]

(17.40)


Using (14.4), the corresponding system-level SPO operator φ is

φ=

(

φC 0φRBSφC φR

)

(17.41)

The new mass matrix of the system is defined by:

M =HφMφ∗H∗ (17.42)

Exercise 17.5 Transformed mass matrix.

1. Show thatφ∗RH

∗RJRG = φ∗

SH∗S (17.43)

2. Show that

θ= JRGθ and φ∗H∗

JRG

= φ∗H∗ where JRG

�=

(

I 0

0 JRG

)

(17.44)

3. Show that the new mass matrix, M is related to the original M mass matrix by

MS = J∗RG

MJRG

(17.45)

4. Verify that the new mass matrix satisfies the following kinetic energy relationship:

Ke =12θ∗Mθ=

12θ∗M θ (17.46)

This is consistent with the intuitive expectation that the kinetic energy should beinvariant with respect to the choice of the base-body.

5. Verify that M and its operators define an SKO model.

17.4 Base-Invariant Operational Space Inertias

The Ω extended operational space compliance matrix for the system is defined in(10.11) and (10.12) as

Ω= φ∗H∗M−1Hφ (17.47)

The following lemma shows that the Ω matrix remains independent of the base-body choice for free-flying systems.

17.4 Base-Invariant Operational Space Inertias 349

Lemma 17.3 Ω is independent of the base-body.TheΩ matrix defined in (17.47) with the nth link as the base-body agrees with thatcomputed when the same definition is used with the kth link as the base-body, i.e.,

Ω= φ∗H∗M−1Hφ (17.48)

Proof: We have

Ω10.12= φ∗H∗M−1Hφ

17.45= φ∗H∗

(J∗RG

MJRG)−1Hφ

17.43= φ∗H∗M−1Hφ

This establishes (17.48).Lemma 10.1 defined the block-diagonal Υ operational space compliance ker-

nel operator as just the block-diagonal elements of the Ω operator. It also de-fined an O(N) base-to-tips scatter algorithm for computing the elements of Υin Algorithm 10.3 on page 195. The following lemma shows that the choiceof base-body used in this scatter algorithm does not alter the elements of Υ.

Lemma 17.4 Base-invariance of Υ.The choice of base-body for a free-flying system used in Algorithm 10.3 for comput-ing the Υ block-diagonal matrix does not affect the computed values.Proof: We have seen in Lemma 17.3 that the value of Ω is independent of thechoice of the base-body. Consequently, its block diagonal element values are inde-pendent of the choice of the base-body. Since the diagonal elements of Υ are indeedthe block-diagonal elements ofΩ, this lemma is established.

The following remark establishes that Υ(k) is always invertible for a free-flyingsystem.

Remark 17.2 Invertibility of Υ(k).As discussed in Remark 10.1 on page 196, for ground-based manipulators, Υ(k) issingular for the first set of links connected to the base. This is because there aredirections along which spatial forces induce no motion in ground-based manipula-tors. In contrast, for free-flying systems, Remark 10.2 on page 196 showed that Υ(.)at the base-body is the inverse of the articulated body inertia, i.e., if link n is thebase-body, then

Υ(n) = P−1(n) (17.49)

Since this relationship holds for any choice of base-body, this implies that Υ(k) isinvertible for all k. This invertibility property is consistent with the property that anynon-zero spatial force at any body on a free-flying manipulator induces a non-zeroacceleration in the body.


The following lemma takes us one step further, and establishes the relationshipbetween Υ(k) and the regular and dual articulated body inertia quantities definedearlier in the chapter. It shows that the symmetry of free-flying manipulators al-lows us to dispense with these algorithms, and express the Υ(k)s directly using theregular and dual articulated body inertias.

Lemma 17.5 Expression for [Υ(k)]−1.

Υ(k) andΥ+(k) can be expressed as follows using the regular and dual articulatedbody inertia quantities:

[Υ+(k)]−1

= P+(k)+S+

(k) ∀ k = 0 · · ·n− 1 (17.50a)

[Υ(k)]−1= P(k)+S(k) ∀ k= 1 · · ·n (17.50b)

Proof: We will prove this lemma using induction. From (17.4) we know thatS(n) = 0. Thus, from (17.49) it follows that

[Υ(n)]−1

= P(n)+S(n)

Having established (17.50b) for link n, we begin the proof by induction by assumingthat (17.50b) is true for link (k+ 1), i.e.,

[Υ(k+ 1)]−1

= P(k+ 1)+S(k+ 1) (17.51)

We will show now that both identities in (17.50) then must hold for link k. We have

[Υ+(k)]−1 10.22

=[

φ∗(k+ 1,k)Υ(k+ 1)φ(k+ 1,k)

]−1

17.3,17.51= φ(k,k+ 1)

[

P(k+ 1)+S(k+ 1)]

φ∗(k,k+ 1)

9.33= P+

(k)+φ(k,k+ 1)[

M(k+ 1)+S(k+ 1)]

φ∗(k,k+ 1)

17.4= P+

(k)+S+(k)

This establishes (17.50a) for the kth link.Now we proceed to show that (17.50b) also must necessarily hold at the kth link.

Now

[P(k)+S(k)]Υ(k)

10.22= [P(k)+S(k)]

[

τ∗(k)Υ+(k)τ(k)+H∗

(k)D−1(k)H(k)

]

17.5=[

P(k)τ∗(k)+S(k)τ∗(k)]

Υ+(k)τ(k)+P(k)H∗

(k)D−1(k)H(k)

9.33=[

P+(k)+S(k)(I−H∗

(k)G∗(k))

]

Υ+(k)τ(k)+τ(k)

17.5=[

P+(k)+S(k)

]

Υ+(k)τ(k)+τ(k)

17.4=[

P+(k)+τdlS

+(k)]

Υ+(k)τ(k)+τ(k)

17.4 Base-Invariant Operational Space Inertias 351

17.4=[

P+(k)+S+

(k)]

Υ+(k)τ(k)−τdlS

+(k)Υ+

(k)τ(k)+τ(k)

17.50a= τ(k)−τdlS

+(k)Υ+

(k)τ(k)+τ(k)

9.33= I−τdlS

+(k)Υ+

(k)τ(k)

17.50a= I−τdl(k)

[

I−P+(k)Υ+

(k)]

τ(k)

9.33= I−τdl(k)τ(k)

[

I−P+(k)Υ+

(k)τ(k)] 17.6

= I

This implies that (17.50b) holds at link k. This concludes the proof by induction.

The positive definiteness of P(.) and S+(.) taken together with the above resultclearly implies that Υ(.) and Υ+(.) are also positive definite (and hence invertible).

Lemma 17.5 provides us with the new Algorithm 17.5 to compute the operationalspace inertias for the links on the free-flying manipulator. Unlike Algorithm 10.3,Algorithm 17.5 has a decoupled structure arising from the base-invariance sym-metry of the free-flying manipulators. The two sequential recursions in the earlieralgorithms are now replaced by a pair of parallel recursions. This can be used to ad-vantage for a parallel implementation. As is the case for serial-chain manipulators,the operational space inertia at any link of a tree-topology free-flying manipulator issimply obtained by summing up the P and S articulated body inertias at the link.

Algorithm 17.5 Decoupled computation of operational space inertias

1. a. Compute the articulated body quantities P(.) recursively from link 1 to link nusing Algorithm 9.5.

b. Simultaneously compute the dual articulated body quantities S+(.) recur-sively from link n to link 1 using Algorithm 17.1.

2. Compute Λ(k) = [P(k)+S(k)] for the kth link. The computations in this stepcan be carried out independently for each link.

We are also now in a position to restate Lemma 17.1 in a more general form usingthe Υ(k) terms.

Lemma 17.6 Alternate expression for the link spatial acceleration.The spatial acceleration α(k) of the kth link is given by:

α(k) = −Υ(k)[

z(k)+ zdl(k)−P(k)a(k)]

α+(k) = −Υ+

(k)[

z+

(k)+ z+dl(k)−S+

(k)adl(k)] (17.52)

Proof: Combine Lemmas 17.1 and 17.5..

Chapter 18Spatial Operator Sensitivities for Rigid-BodySystems

In this chapter, we derive analytical operator expressions for the time derivativesand sensitivities of several spatial operator expressions with respect to generalizedcoordinates for tree-topology rigid-link multibody systems. Such sensitivities haveapplications such as for diagonalization [84], numerical integration, optimization,Christoffel symbol computations, control [62] and model linearization [81, 85, 116,117].

18.1 Preliminaries

18.1.1 Notation

For a stacked spatial vector, X�= col

{X(k)

}n

k=1, define the notation

XS

�= col

{X(℘(k))

}n

k=1

X�= diag

{X(k)

}n

k=1, and ˜X

�= diag

{˜X(k)

}n

k=1(18.1)

In particular, XS has the following form:

XS

�= diag

{X(℘(k))

}n

k=1=

∑j=1,n

∑k∈�(j)

ekX(j)e∗k (18.2)

Recall that

Vω(k)

1.21=

[

ω(k)

0

]

⇒ ˜Vω(k)

1.23=

(

ω(k) 0

0 ω(k)

)


354 18 Spatial Operator Sensitivities for Rigid-Body Systems

Vv(k)

1.21=

[

0

v(k)

]

⇒ Vv(k)

1.25=

(

0 v(k)

0 0

)

Using the notational definitions in (18.1), we have

Vω �= col

{Vω

(k)}n

k=1, Vv �

= col{Vv

(k)}n

k=1

and ΔωV

�= col

{Δω

V (k)}n

k=1, Δv

V�= col

{Δv

V(k)}n

k=1(18.3)

Furthermore,

V�= diag

{V(k)

}n

k=1and VS

�= diag

{V(℘(k))

}n

k=1

˜VωS

�= diag

{˜Vω

(℘(k))}n

k=1

18.2=

∑j=1,n

∑k∈�(j)

ek˜Vω

(j)e∗k

VvS

�= diag

{V

v(℘(k))

}n

k=1

18.2=

∑j=1,n

∑k∈�(j)

ek Vv(j)e∗k (18.4)

Also,

Vω= Vω

S +ΔωV ,

[

˜Vω]∗

= − ˜Vω,[

˜VωS

]∗= − ˜Vω

S(18.5)

18.1.2 Identities for Vv

Lemma 18.1 Identities for Vv.The following identities hold for Vv:

VvEφ = Eφ V

vS (18.6a)

φVv− V

vSφ = φ

[

Vv− V

vS

]

φ (18.6b)

φVvS − V

vSφ = φ

[

Vv− V

vS

]

φ (18.6c)

Proof: At the component level we have

φ(℘(k),k)Vv(k)

1.36e= V

v(k)

Vv(℘(k))φ(℘(k),k)

1.36e= V

v(℘(k)) (18.7)

18.1 Preliminaries 355

For (18.6a),

Eφ VvS

18.2=

n∑k=1

n∑j=1

∑i∈�(j)

e℘(k)φ(℘(k),k)e∗kei Vv(j)e∗i

=

n∑k=1

e℘(k)φ(℘(k),k)Vv(℘(k))e∗k

18.7=

n∑k=1

e℘(k) Vv(℘(k))e∗k

18.7=

n∑k=1

e℘(k) Vv(℘(k))φ(℘(k),k)e∗k

8.22=

n∑k=1

e℘(k) Vv(℘(k))e∗℘(k)e℘(k)φ(℘(k),k)e∗k (18.8)

Also,

VvEφ

17.23,18.2=

n∑k=1

∑i=1,n

ei Vv(i)e∗i e℘(k)φ(℘(k),k)e∗k

=

n∑k=1

e℘(k) Vv(℘(k))e∗℘(k)e℘(k)φ(℘(k),k)e∗k

Comparing this with (18.8) establishes (18.6a).To establish (18.6b), pre- and post-multiply (18.6a) with φ to yield

0 = φEφ VvSφ−φV

vEφφ= φV

vSφ−φV

vφ

= φVvSφ− V

vSφ−φV

vφ+φV

v= φV

v− V

vSφ−φ

[

Vv− V

vS

]

φ

For (18.6c), we have

φVv− V

vSφ

18.6b= φ

[

Vv− V

vS

]

φ= φ[

Vv− V

vS

]

φ+φ[

Vv− V

vS

]

Rearranging terms establishes the result.

Exercise 18.1 Identities for ˜V .Establish the following identities:


E∗φ

˜V = ˜V+ E∗φ (18.9a)

˜VE∗φ −E∗

φ˜V = ˜ΔV E∗

φ (18.9b)

˜Vφ∗−φ∗

˜V = φ∗˜ΔV φ

∗ (18.9c)

φV− Vφ = φΔVφ (18.9d)

18.2 Operator Time Derivatives

We now derive expressions for the inertial frame time derivatives of various spatialoperators. For simplicity of exposition, we make the following assumption.

Assumption 18.1 (Operator Time Derivatives).

• The multibody system is a tree-topology system.• All the bodies in the system are rigid bodies.• All the time derivatives are inertial time derivatives – and, for simplicity, the

notation will not state this explicitly.• The kth body frame Bk coincides with the Ok hinge frame. Section 18.2.2 dis-

cusses the case when this is true, but by default our assumption is that the framescoincide.

• The H∗(k) joint map matrix is constant in the body frame of the child body.

We will make extensive use of (1.28) which relates the time derivatives of spatialvectors in different frames.

18.2.1 Time Derivatives of φ(k+1,k), H(k) and M(k)

Lemma 18.2 Time derivatives of φ(k+ 1,k), H(k) and M(k).We have that

φ(℘(k),k) = Vv(k)− V

v(℘(k)) (18.10a)

= ˜Vω(℘(k))φ(℘(k),k)

−φ(℘(k),k) ˜Vω(℘(k))+ ΔvV (18.10b)

= V(℘(k))φ(℘(k),k)

−φ(℘(k),k)V(℘(k))+ ΔvV (18.10c)

H∗(k) = ˜Vω

(℘(k))H∗(k) (18.10d)

M(k) = ˜Vω(k)M(k)−M(k)˜Vω

(k) (18.10e)

18.2 Operator Time Derivatives 357

Proof: Equation (18.10a) derives directly from Exercise 1.8 on page 13, whichestablished that

dl(℘(k),k)dt

= v(k)−v(℘(k)) (18.11)

However, an alternate expression for (18.11) that does not depend upon v(℘(k)) isgiven by:

dl(℘(k),k)dt

5.13= Δv(k)+ ω(℘(k))l(℘(k),k)

Hence,

φ(℘(k),k) =

(

0 [Δv(k)+ ω(℘(k))l(℘(k),k)]∼

0 0

)

=

(

0 ˜Δv(k)+ ω(℘(k))˜l(℘(k),k)−˜l(℘(k),k)ω(℘(k))

0 0

)

= ˜Vω(℘(k))φ(℘(k),k)−φ(℘(k),k) ˜Vω

(℘(k))+ ΔvV(k)

This establishes (18.10b).For (18.10c),

˜Vω(℘(k))φ(℘(k),k)−φ(℘(k),k) ˜Vω

(℘(k))

=

(

V(℘(k))− Vv(℘(k))

)

φ(℘(k),k)−φ(℘(k),k)(

V(℘(k))− Vv(℘(k))

)

18.7= V(℘(k))φ(℘(k),k)− V

v(℘(k))−φ(℘(k),k)V(℘(k))+ V

v(℘(k))

= V(℘(k))φ(℘(k),k)−φ(℘(k),k)V(℘(k))

Equation (18.10d) follows from (1.28), and thatH∗(k) is constant in the body frameof the child body.

Equation (18.10e) is a restatement of (2.22).

Lemma 18.3 Time derivatives of spatial operators.We have

Eφ = ˜Vω Eφ −Eφ

(

˜VωS − Δ

vV

)

= ˜Vω Eφ −Eφ˜Vω

+EφΔV (18.12a)

H∗

= ˜VωS H

∗ (18.12b)

M = ˜Vω M−M˜Vω (18.12c)

φ = φΔVφ+ ˜Vωφ−φ˜Vω (18.12d)

Proof: Using (8.22), the first expression in (18.12a) is an operator level restate-ment of the component level relationships in (18.10b). The second expression fol-lows by further use of the following relationships:


Vω= Vω

S +ΔωV and ΔV = Δω

V +ΔvV

Equations (18.12b) and (18.12c) are simply operator level restatements of (18.10d)and (18.10e), respectively.

For (18.12d),

φA.28= −φ

dφ−1

dtφ= −φ

d(I−Eφ)

dtφ= φEφφ

18.12a= φ

[

˜Vω Eφ −Eφ˜Vω

+EφΔvV

]

φ= φ˜Vω φ− φ˜Vωφ+ φΔvVφ

= ˜Vωφ−φ˜Vω+ φΔ

vVφ

This lemma shows that the time derivatives of the spatial operators can them-selves be analytically expressed in terms of the operators themselves!

Exercise 18.2 Time derivative of Hφ.Show that

dHφdt

=Hφ[

˜ΔωV φ+EφΔ

vVφ− ˜Vω

]

(18.13)

Exercise 18.3 Operator expression for the aI Coriolis acceleration.

1. Use (18.13) to derive the following operator expression for the inertial framederivative Coriolis acceleration aI defined in Exercise 5.3:

aI = ˜VωS V−E∗

φ˜Vω V− ˜Δv

V V+ (18.14)

2. Verify that the aI(k) component elements in (18.14) agree with the ones in (5.20)on page 81.

18.2.2 Time Derivatives with Ok �= Bk

While Lemma 18.2 assumes that the body frame coincides with the Ok frame, Ex-ercise 18.4 addresses the situation when the pair of frames do not coincide for thetime derivatives of the link-level component terms. Exercise 18.5 addresses exten-sions for the time derivatives of the corresponding spatial operators. The expressionfor the time derivative ofM(k) in (18.10e) is valid even in the case where the framesdo not coincide.

18.2 Operator Time Derivatives 359

Exercise 18.4 Time derivative of φ(k+ 1,k) with Ok �= Bk.In general, the body frame is not at the hinge location, i.e., Bk �= Ok. For this case,we have from (3.23) that

φ(k+ 1,k)3.23= φ(B℘(k),Bk)

= φ(℘(k),O℘(k))φ(O℘(k),Ok)φ(Ok,k) (18.15)

Define

VB/O(k)�=

[

0v(Ok)−v(Bk)

]

(18.16)

1. Show thatdφ(Bk,Ok)

dt= VB/O(k) (18.17)

2. Show that

φ(k+ 1,k) = Vv(k)− V

v(℘(k)) (18.18a)

= φ(℘(k),O℘((k))) φ(O℘(k),Ok) φ(Ok,k)

+VB/O(℘(k)) φ(O℘(k),k)

−φ(℘(k),Ok) VB/O(k) (18.18b)

= ˜Vω(k+ 1)φ(k+ 1,k)−φ(k+ 1,k)˜Vω

(k+ 1)

+ΔvV(k)+ ˜Δω

V (k)φ(Ok,k)−φ(Ok,k) ˜ΔωV (k) (18.18c)

= V(k+ 1)φ(k+ 1,k)−φ(k+ 1,k)V(k+ 1)

+ΔvV(k)+ ΔV(k)φ(Ok,k)−φ(Ok,k)ΔV(k) (18.18d)

Equation (18.10) provides the expression for φ(O℘(k),Ok) required in (18.18b).

It is easy to verify that the expressions for φ(k+ 1,k) and φ(O℘(k),Ok) agreewhen the kth body frame Bk coincides with Ok.

The following exercise generalizes the operator time derivative expressions inLemma 18.3 to handle the case where Bk does not coincide with Ok.

Exercise 18.5 Operator time derivatives with Ok �= Bk.Define

VB/O

�= col

{VB/O(k)

}n

k=1(18.19)

Recall from (5.28) that

ΔB/O

�= diag

{φ(Bk,Ok)

}n

k=1(18.20)


1. Show that

dΔB/O

dt= VB/O (18.21a)

dΔ−1B/O

dt= −VB/O (18.21b)

2. Show that

dEφB

dt= ΔB/OEφΔ

−1B/O

+ VB/OEφB−EφB

VB/O (18.22a)

dφB

dt= ΔB/OφΔ

−1B/O

+ VB/OφB −φB VB/O (18.22b)

dHB

dt= HΔ−1

B/O−HB VB/O (18.22c)

18.2.3 Time Derivative of the Mass Matrix

Having developed expressions for the time derivatives of the component spatial op-erators, an expression for the time derivative of the mass matrix is derived in thefollowing lemma.

Lemma 18.4 Time derivative of the mass matrix.The expression for the time derivative of the mass matrix is given by

M(θ) =Hφ[(

˜ΔωV +EφΔ

vV

)

φM

−Mφ∗(

˜ΔvV E∗

φ + ˜ΔωV

)]

φ∗H∗(18.23)

Proof: The time derivative of the mass matrix is given by

M8.45=

dHφdt

Mφ∗H∗+Hφ

dM

dtφ∗H∗

+HφMdφ∗H∗

dt18.13,18.12c

= Hφ[

˜ΔωV φ+EφΔ

vVφ− ˜Vω

]

Mφ∗H∗

+Hφ[

˜Vω M−M˜Vω]

φ∗H∗

+HφM[

−φ∗˜Δω

V −φ∗˜Δv

V E∗φ + ˜Vω

]

φ∗H∗

=Hφ[(

˜ΔωV +EφΔ

vV

)

φM−Mφ∗(

˜ΔωV + ˜Δv

V E∗φ

)]

φ∗H∗

establishing (18.23).

18.3 Operator Sensitivities 361

Exercise 18.6 Alternative derivation of M expression.

1. Show that

dφMφ∗

dt=

[

φΔV + ˜Vω]

φMφ∗−φMφ∗

[

˜ΔV φ∗+ ˜Vω

]

(18.24)

2. Use (18.24) to derive the (18.23) expression for M.

In the special case that the hinges in the system have no prismatic components,˜Δv

V = 0, which further simplifies the various time derivative expressions in thissection. From the expression in (18.23), we can see that the composite body iner-tia based decomposition techniques discussed in Sect. 9.3.2 can be used to developefficient computational algorithms for computing the time derivative of the massmatrix.

18.3 Operator Sensitivities

Our goal now is to develop analytical expressions for the derivatives of variousspatial operator expressions with respect to generalized coordinates. We add thefollowing assumption to the ones at the beginning of Sect. 18.2:

• All hinges in the system are 1 degree of freedom hinges.

This assumption helps simplify the notation for the sensitivity expressions. Thereis little loss in generality, since multiple degree of freedom hinges can always beexpressed as a sequence of 1 degree of freedom hinges.

Our approach will build upon the time derivatives that have already been derivedin the earlier sections. To do this, we use the following property of derivatives from(A.23) on page 401, which states that, for an arbitrary function g(θ, θ),

∂g(θ, θ)

∂θ=

ddt∂g

∂θ+∂g

∂θ(18.25)

For the case where the function is independent of θ, i.e., g(θ, θ) = g(θ), (18.25)simplifies to

∂g(θ)

∂θ=∂g(θ)

∂θ(18.26)

Equation (18.26) provides a path for obtaining the sensitivity of operator expres-sions (since they are functions of θ only) from the partial derivatives of the timederivatives of the spatial operators. The advantage of this approach is that, while thespatial operators are complex, non-linear functions of the generalized coordinates,their time derivatives have simpler linear dependency on the generalized velocities.


Thus, evaluating the left-hand side of (18.26) is much easier compared to directlyevaluating the right-hand side. We begin by deriving the partials of various body ve-locities with respect to generalized velocities, and introducing additional convenientnotation in the next section.

18.3.1 The ˜Hω

�i , ˜Hω

≺i , and ˜Hω

=i Operators

We can use (3.8) to express a joint map matrix in the form:

H∗(k) =H∗

ω(k)+H∗v(k)

where H∗ω(k)

�=

[

hω(k)

0

]

, H∗v(k)

�=

[

0

hv(k)

]

(18.27)

Lemma 18.5 Component-level velocity partials.The following expressions describe the component-level partial derivatives of bodyand joint relative spatial velocities:

∂Vω(k)

∂θi

= H∗ω(i) ·1[i�k] (18.28a)

∂Vω(℘(k))

∂θi

= H∗ω(i) ·1[i�k] (18.28b)

∂ΔωV (k)

∂θi

= H∗ω(i) ·1[i=k] (18.28c)

∂ΔvV(k)

∂θi

= H∗v(i) ·1[i=k] (18.28d)

∂ΔV(k)

∂θi

= H∗(i) ·1[i=k] (18.28e)

1[<...>] denotes the indicator function introduced in (8.2) on page 138.

Proof: The above expressions follow directly from taking partials of the follow-ing basic expressions obtained from (3.8), (18.27) and (3.19b):

ΔωV (k) =H∗

ω(k)θk, ΔvV(k) =H∗

v(k)θk, Vω(k) =

∑j�k

ΔωV (j)

The following exercise derives expressions for the partial derivative of the com-ponent level quantities such asφ(℘(k)) etc., with respect to generalized coordinates.We will use the notational shorthand Xθi

for ∂X∂θi

.

18.3 Operator Sensitivities 363

Exercise 18.7 Sensitivities of φ(℘(k),k), H(k) and M(k).Show that

[φ(℘(k),k)]θi=

[

˜H∗ω(i)φ(℘(k),k)−φ(℘(k),k) ˜H∗

ω(i)]

·1[k≺i]

+H∗v(k) ·1[k=i]

=

⎧⎪⎨⎪⎩H

∗v(k) for k= i

˜H∗ω(i)φ(℘(k),k)−φ(℘(k),k) ˜H∗

ω(i) for k≺ i0 otherwise

(18.29a)

[H∗(k)]θi

= ˜H∗ω(i)H∗

(k) ·1[k≺i]

=

⎧⎪⎨⎪⎩

[

˜hω(i)hω(k)

˜hω(i)hv(k)

]

for k≺ i

0 otherwise

(18.29b)

[M(k)]θi= [ ˜H∗

ω(i)M(k)−M(k) ˜H∗ω(i) ] ·1[k�i]

=

{˜H∗

ω(i)M(k)−M(k) ˜H∗ω(i) for k� i

0 otherwise(18.29c)

We use this component-level lemma to derive expressions for the partials ofstacked spatial velocities. For a matrix or vector, X, we use the following notationto derive stacked vectors from it:

X=i�= col

{X ·1[i=k]

}n

k=1

X�i�= col

{X ·1[k�i]

}n

k=1

X≺i�= col

{X ·1[k≺i]

}n

k=1

(18.30)

Thus, the stacked vector X=i contains only a single non-zero entry, X, at the ithslot. X≺i contains the non-zero element X at all slots for whom the ith body is astrict ancestor, while X�i contains X additionally at the ith slot.

Lemma 18.6 Operator-level velocity partials.The following expressions describe partial derivatives of stacked spatial velocityvectors:


∂Vω

∂θi

= col{H∗

ω(i) ·1[k�i]

}n

k=1

�= Hω

�i (18.31a)

∂VωS

∂θi

= col{H∗

ω(i) ·1[k≺i]

}n

k=1

�= Hω

≺i (18.31b)

∂ΔωV

∂θi

= col{H∗

ω(i) ·1[k=i]

}n

k=1

�= Hω

=i (18.31c)

∂ΔvV

∂θi

= col{H∗

v(i) ·1[k=i]

}n

k=1

�= Hv

=i (18.31d)

∂ΔV

∂θi

= col{H∗

(i) ·1[k=i]

}n

k=1

�= H=i (18.31e)

Proof: The expressions in (18.31) are simply stacked vector, equivalent versionsof the component-level expressions in (18.28).

For the newly defined Hω=i , Hv

=i and H=i , block-vectors, it is easy to verifythat

˜ΔωV Hω

=i = 0, ˜ΔvV Hv

=i = 0, ˜ΔV H=i = 0 (18.32)

Lemma 18.7 Operator sensitivities of φ, H, M.

[Eφ]θi= ˜H

ω

�i Eφ −Eφ˜H

ω

≺i +Eφ Hv=i (18.33a)

[φ]θi= φH=iφ−φ ˜H

ω

�i + ˜Hω

�iφ (18.33b)

[φ]θi(k, j) = φ(k, i) ˜H∗

ω(i)φ(i, j) ·1[k�i] −φ(k, j) ˜H∗ω(i) ·1[k�i]

+ ˜H∗ω(i)φ(k, j) ·1[k�i] (18.33c)

[H∗]θi

= ˜Hω

≺iH∗ (18.33d)

[M]θi= ˜H

ω

�i M−M ˜Hω

�i (18.33e)

Proof: The expressions in (18.33) follow directly from applying (18.26) to thetime derivative expressions in (18.12), together with the expressions in (18.31).

Exercise 18.8 Sensitivity of Hφ.Show that

[Hφ]θi=Hφ

[

˜Hω

=iφ+Eφ Hv=iφ− ˜H

ω

�i

]

(18.34)

18.4 Mass Matrix Related Quantities 365

18.4 Mass Matrix Related Quantities

18.4.1 Sensitivity of φMφ∗

Lemma 18.8 Sensitivity of φMφ∗.

[φMφ∗]θi

=

[

φH=i + ˜Hω

�i

]

φMφ∗

−φMφ∗[

˜H=i φ∗+ ˜H

ω

�i

] (18.35)

Proof: Equation (18.35) follows from applying (18.26) to the time derivative ex-pressions in (18.24).

18.4.2 Sensitivity of the Mass Matrix Mθi

The sensitivity of the mass matrix with respect to a generalized coordinate is de-scribed in the following lemma.

Lemma 18.9 Sensitivity of the mass matrix.The sensitivity of the mass matrix M(θ) is given by the following expression:

Mθi=Hφ

[(

˜Hω

=i +Eφ Hv=i

)

φM

−Mφ∗(

˜Hω

=i + ˜Hv

=i E∗φ

)]

φ∗H∗(18.36)

Proof: Equation (18.36) follows from applying (18.26) to the time derivative ex-pressions in (18.23).

The expression in (18.36) is closed-form, and expresses the mass matrix sensitiv-ity in terms of the operators φ, M, and H appearing in the mass matrix itself. Theanalytical form implies that the mass matrix derivatives can be easily computed us-ing operations and spatially recursive algorithms, similar to those used to computethe mass matrix itself. As discussed in Chap. 19, the mass matrix sensitivity expres-sion allows us to develop simple closed-form expressions and recursive algorithmsfor the Coriolis term in the diagonalized equations of motion.

Remark 18.1 Expression for MD(θ, θ).Equation (4.29) on page 67 defines the mass matrix-related gradient of the general-ized momentum asMD(θ, θ) as

MD(θ, θ) = ∇θ(Mθ) = [Mθ1 θ, Mθ2 θ, · · · Mθn θ]


The ith column of MD is, thus, simply Mθiθ, which from (18.36) has the form:

Mθiθ

18.36= Hφ

[(

˜Hω

=i +Eφ Hv=i

)

φM

−Mφ∗(

˜Hω

=i + ˜Hv

=i E∗φ

)]

φ∗H∗ θ

=Hφ[(

˜Hω

=i +Eφ Hv=i

)

φM

−Mφ∗(

˜Hω

=i + ˜Hv

=i E∗φ

)]

V

(18.37)

18.4.3 Sensitivity of the Kinetic Energy

In this section, we derive expressions for the gradient of the system kinetic energy,Ke, with respect to the generalized coordinates.

Lemma 18.10 Operator expression for ∂Ke∂θ .

∂Ke

∂θ= −H

[

V− ˜ΔωV

]

φMV (18.38a)

= −Hφ{

V−

(

EφΔvV + ˜Δω

V

)

φ}

MV (18.38b)

= −Hφ{

VM−

(

EφΔvV + ˜Δω

V

)

[I+φKH]P}V (18.38c)

Proof: We have,

∂Ke

∂θi

4.5=

12∂θ

∗Mθ

∂θi=

12θ∗Mθi

θ

18.36=

12V∗

[(

˜Hω

=i +Eφ Hv=i

)

φM−Mφ∗(

˜Hω

=i + ˜Hv

=i E∗φ

)]

V

= V∗(

˜Hω

=i +Eφ Hv=i

)

φMV

8.42=

(

V∗˜H

ω

=i +(V−ΔV)∗ H

v=i

)

φMV

18.27=

(

V∗( ˜H

ω

=i + Hv=i )− (ΔV)

∗ Hv=i

)

φMV

18.3=

(

V∗ H=i −(ΔωV +Δv

V)∗ H

v=i

)

φMV

18.32=

(

V∗ H=i −(ΔωV )

∗ Hv=i

)

φMV

18.4 Mass Matrix Related Quantities 367

18.32=

(


∗( H

v=i + ˜H

ω

=i )

)

φMV

18.31=

(


∗ H=i

)

φMV

= (V−ΔωV )

∗H=iφMV

Hence,

∂Ke

∂θ= col

{∂Ke

∂θi

}n

i=1= diag

{(V(i)−Δω

V (i))∗H

∗(i)

}n

i=1φMV

1.27a= −diag

{H(i)

(

V(i)− ˜ΔωV (i)

)}n

i=1φMV

= −Hdiag{(

V(i)− ˜ΔωV (i)

)}n

i=1φMV = −H

(

V− ˜ΔωV

)

φMV

This establishes (18.38a). For (18.38b),

H[

V− ˜ΔωV

]

φMV =H[

Vφ− ˜ΔωV φ

]

MV

18.9d= H

[

φV− φΔVφ− ˜ΔωV φ

]

MV

=Hφ[

V−EφΔVφ−(I−Eφ) ˜ΔωV φ

]

MV

=Hφ[

V−

(

Eφ

(

ΔV − ˜ΔωV

)

+ ˜ΔωV

)

φ]

MV

=Hφ[

V−

(

EφΔvV + ˜Δω

V

)

φ]

MV

Using this in (18.38a) establishes (18.38b). Equation (18.38c) follows directly fromusing (9.48) in (18.38b).

18.4.4 Equivalence of Lagrangian and Newton–Euler Dynamics

We now prove that the equations of motion obtained using the system-level La-grangian formulation in Sect. 4.2, and the ones obtained from the Newton–Eulercomponent-level approach in Sect. 5.1.2, are equivalent. There are two parts to es-tablishing the equivalency:

1. First, we need to show that the mass matrices from the two approaches are equiv-alent. The mass matrix from the Lagrangian approach, as established by (4.5),(4.6), (4.25) (4.26) is M =HφMφ∗H∗. This is precisely the expression for themass matrix from the Newton–Euler approach in (5.25). So the mass matricesare indeed equivalent.

2. The second part requires establishing that the Coriolis generalized forces termdefined in (4.27) via the Lagrangian approach agrees with the one in (5.25) usingthe Newton–Euler approach. Proving this is the subject of the following exercise.


Exercise 18.9 Equivalence of Lagrangian and Newton–Euler equa-tions of motion.In this exercise, we will work with the “inertial derivatives” form of the equationsof motion, and make this explicit with the use of the I subscript.

1. In the Newton–Euler approach, the expression for the Coriolis generalized forcesvector, C(θ, θ), is derived in (5.25) as

C(θ, θ)�= Hφ [bI +Mφ∗

aI] (18.39)

where the stacked vectors bI and aI are defined as

bI�= col

{bI(k)

}n

k=1and aI

�= col

{aI(k)

}n

k=1(18.40)

The body-level bI(k) gyroscopic spatial force is defined in Lemma 2.2 on page 27for the inertial derivative rigid body equations of motion, while the body-levelaI(k) Coriolis spatial acceleration are defined in (5.19) on page 80. Show thatC(θ, θ) in (18.39) can be expressed as:

C(θ, θ) =Hφ[

VM−Mφ∗(

˜ΔvV E∗

φ + ˜ΔωV

)]

V (18.41)

2. Equation (4.27) defines the Coriolis generalized forces expression, C(θ, θ), fromthe Lagrangian approach as

C(θ, θ) = M(θ)θ−12∂[θ

∗M(θ)θ]

∂θ∈RN

Show that this expression is equivalent to the expression in (18.41).

This establishes the equivalency of the Coriolis generalized acceleration obtainedvia the Lagrangian and the Newton–Euler approaches. Silver [167] studied thisequivalency using a component-level approach for serial-chain manipulators.

18.5 Time Derivatives of Articulated Body Quantities

We now turn to the topic of computing the time derivatives of the articulated bodyinertia operators. The time-derivative of the articulated body inertia quantities areobtained in two steps. First, we develop expressions for the time-derivatives of thearticulated body inertia quantities in Lemma 18.11 assuming that P is known. Fol-lowing this, we address the issue of obtaining analytical expressions for P itself.

Lemma 18.11 Time derivatives of articulated body quantities.Define the block-diagonal, symmetric operator λ in terms of P as follows:

18.5 Time Derivatives of Articulated Body Quantities 369

λ�= P− ˜Vω

S P+P ˜VωS

(18.42)

The time derivatives of the articulated body inertia related quantities are given bythe following expressions:

D = HλH∗ (18.43a)

D−1

= −D−1HλH∗D−1 (18.43b)

G = τλH∗D−1+ ˜Vω

S G (18.43c)

τ = τλH∗D−1H+ ˜VωS τ−τ ˜Vω

S (18.43d)

τ = −τλH∗D−1H− ˜VωS τ+τ ˜Vω

S (18.43e)

Eψ = ˜Vω Eψ −Eψ˜Vω

S −Eφ

(

τλH∗D−1H− ΔvV τ

)

(18.43f)

P+

= τλτ∗ + ˜VωS P+

−P+˜Vω

S (18.43g)

Proof: Since D =HPH∗, we have:

D = HPH∗+HPH∗

+HPH∗ 18.12b

= −H ˜VωS PH∗

+HPH∗+HP ˜Vω

S H∗

18.42= HλH∗

This establishes (18.43a).For (18.43b), we have

D−1 A.28

= −D−1DD−1 18.43a= −D−1HλH∗D−1

For (18.43c),

G = PH∗D−1+PH

∗D−1

+PH∗D−1

18.12b,18.43b= PH∗D−1

+P ˜VωS H

∗D−1−PH∗D−1HλH∗D−1

=

(

P+P ˜VωS

)

H∗D−1−τλH∗D−1 18.42

=

(

λ+ ˜VωS P

)

H∗D−1−τλH∗D−1

= τλH∗D−1+ ˜Vω

S G

Furthermore, for (18.43d):

τ= GH+GH18.12b,18.43c

=

(

τλH∗D−1+ ˜Vω

S G)

H−GH ˜Vω

= τλθiH∗D−1H+ ˜Vω

S τ−τ ˜VωS

Equation (18.43e) follows from using τ= I−τ together with (18.43d).


For (18.43f), we have

Eψ = Eφτ+Eφ τ

18.12a,18.43e=

(

˜Vω Eφ −Eφ˜Vω

S +EφΔvV

)

τ

+Eφ

(

−τλH∗D−1H− ˜VωS τ+τ ˜Vω

S

)

= ˜Vω Eψ +(−Eφ˜Vω

S τ−Eφ˜Vω

S τ+Eφτ ˜VωS )

+EφΔvV τ−EφτλH

∗D−1H ˜VωS

= ˜Vω Eψ(−Eφ˜Vω

S +Eφτ ˜VωS )+EφΔ

vV τ−EφτλH

∗D−1H

= ˜Vω Eψ −Eψ˜Vω

S +EφΔvV τ−EφτλH

∗D−1H

This establishes (18.43f).Since P+ = τP,

P+

= τP+τP =

(

−τλH∗D−1H− ˜VωS τ+τ ˜Vω

S

)

P+τP

18.42= −τλτ− ˜Vω

S τP+τ ˜VωS P+τ

(

λ+ ˜VωS P−P ˜Vω

S

)

= τλτ− ˜VωS τP+ ˜Vω

S P−τP ˜VωS

= τλτ+ ˜VωS τP−τP ˜Vω

S = τλτ+ ˜VωS P+

−P+˜Vω

S

This establishes (18.43g).

Lemma 18.12 The λ block-diagonal operator.The block-diagonal operator λ defined in (18.42) satisfies the following forwardLyapunov equation:

λ−EψλE∗ψ =

(

˜ΔωV P−P ˜Δω

V

)

+Eφ

(

ΔvV P+

−P+˜Δv

V

)

E∗φ

(18.44)

Proof: First, let us verify that the forward Lyapunov equation has a well-defined,block-diagonal solution. This follows from Lemma 9.3 on page 166, whose require-ments are met since Eψ is an SKO operator, and the expression on the right of(18.44) is block-diagonal. Now, recall the articulated body inertia Riccati equation:

0 9.36= P−M−EψPE∗

ψ(18.45)

Differentiating this equation with respect to time leads to

18.5 Time Derivatives of Articulated Body Quantities 371

0 = P− M− EψPE∗ψ −EψPE∗

ψ −EψPE∗ψ

18.12c,18.43f= P−

(

˜Vω M−M˜Vω)

−EψPE∗ψ

−

(

˜Vω Eψ −Eψ˜Vω

S −Eφ

(


))

PE∗ψ

−EψP(

−E∗ψ

˜Vω+ ˜Vω

S E∗ψ −

(

H∗D−1Hλτ∗ +τ∗ ˜ΔvV

)

E∗φ

)

= P− ˜Vω M+M˜Vω− ˜Vω EψPE∗

ψ +EψPE∗ψ

˜Vω

−Eψ

(

P+ ˜VωS P−P ˜Vω

S

)

E∗ψ

+Eφ

(


)

PE∗ψ

+EψP(

H∗D−1Hλτ∗ +τ∗ ˜ΔvV

)

E∗φ

18.42,18.45= P− ˜Vω M+M˜Vω

− ˜Vω(P−M)+ (P−M)˜Vω

−EψλE∗ψ +Eφ

(

τλτ∗τ∗ − ΔvV P+

+ττλτ∗ +P+˜Δv

V

)

E∗φ

= P− ˜Vω P+P ˜Vω−EψλE

∗ψ +Eφ

(

−ΔvV P+

+P+˜Δv

V

)

E∗φ

18.42= λ− ˜Δω

V P+P ˜ΔωV −EψλE

∗ψ +Eφ

(

−ΔvV P+

+P+˜Δv

V

)

E∗φ

This establishes (18.44).We now group sub-expressions in (18.44) to introduce intermediate block-

diagonal operators P and P+ as follows:

λ =

(

˜ΔωV P−P ˜Δω

V

)

+Eφ

(

τλτ∗︸︷︷︸P+

+ΔvV P+

−P+˜Δv

V

︸︷︷︸P+

)

E∗φ

︸︷︷︸P︸︷︷︸

λ

(18.46)

Thus, λ can be broken down into the following sequence of sub-expressions:

P+ �= τλτ∗

P+ �= P+

+ ΔvV P+

−P+˜Δv

V

P�= EφP+E∗

φ

λ = P+ ˜ΔωV P−P ˜Δω

V (18.47)

The expressions in (18.47) can be mapped into the O(N) tips-to-base recursivegather Algorithm 18.1 for computing the elements of the block-diagonal λ.


Algorithm 18.1 Computation of λ

The diagonal elements of the λ block-diagonal operator can be computed using thefollowingO(N) tips-to-base gather recursive algorithm:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

P+(tips) = 0

for k = 1 · · ·nP(k) =

∑j∈�(k)

φ(k, j)P+(j)φ∗

(k, j)

λ(k) = P(k)+ ˜ΔωV (k)P(k)−P(k) ˜Δω

V (k)

P+(k) = τ(k)λ(k)τ∗(k)

P+(k) = P+

(k)+ ΔvV(k)P+

(k)−P+(k) ˜Δv

V(k)

end loop

(18.48)

Once λ is available, (18.42) leads us to the following expression for P:

P = λ+ ˜VωS P+P ˜Vω

S(18.49)

The following lemma describes the component expressions for the time derivativesof the articulated body inertia quantities.

Lemma 18.13 Component-level time derivatives of articulated bodyquantities.

P(k) = ˜Vω(℘(k))P(k)−P(k)˜Vω(℘(k))+ λ(k)

P+

(k) = ˜Vω(℘(k))P+(k)−P+

(k) ˜Vω(℘(k))

+τ(k)λ(k)τ∗(k)

D(k) =H(k)λ(k)H∗(k)

G(k) = ˜Vω(℘(k))G(k)+τ(k)λ(k)H∗

(k)D−1(k)

τ(k) = ˜Vω(℘(k))τ(k)−τ(k)˜Vω(℘(k))

+τ(k)λ(k)H∗(k)D−1

(k)H(k)

ψ(℘(k),k) = ˜Vω(℘(k))ψ(℘(k),k)−ψ(℘(k),k)˜Vω(℘(k))

+φ(℘(k),k)(

ΔvV(k)τ(k)

−τ(k)λ(k)H∗(k)D−1

(k)H(k))

(18.50)

Proof: These expressions are the component-level relationships corresponding tothe operator level expressions in (18.42) and (18.43).

18.6 Sensitivity of Articulated Body Quantities 373

The process for computing the time derivatives of the articulated body inertiaoperators can be summarized as follows:

1. Compute the articulated body inertia quantities recursively using the tips-to-basegather recursion in (9.33) on page 175.

2. Compute λ using the tips-to-base gather recursion in Algorithm 18.1.3. Use λ to compute P using (18.49).4. Use λ in (18.43) to compute the time derivatives of the other articulated body

inertia operators.

The above steps can be combined into a single tips-to-base gather recursion to com-pute all the articulated body inertia time derivatives. This follows from a closer lookat the component level time derivative expressions in (18.50). The component-leveltime-derivative quantities for a body depend only on the articulated body inertia val-ues for the body and those of children bodies, and these are all available when theregular articulated body inertia gather recursion reaches the body. This implies thatthe regular articulated body inertia gather recursion (Step 1 above) can be extendedso that the step at the kth body also computes λ(k) and the articulated body inertiaquantity time-derivatives for the kth body using the expressions in (18.50).

Exercise 18.10 Physical interpretation of λ.With

P(k)�=

dIP(k)

dtand P

+(k)

�=

dIP+(k)

dt

show that the following are true:

λ(k) =d

O+kP(k)

dt, P(k) =

dOkP(k)

dt, P+

(k) =d

O+kP+(k)

dt(18.51)

Thus, while P(k) is the time derivative of P(k) with respect to the inertial frameI, P(k) is its time derivative with respect to the Ok frame, and λ(k) is its time

derivative with respect to the O+k frame. Similarly, while P

+(k) is the time deriva-

tive of P+(k) with respect to the inertial frame I, P+(k) is its time derivative withrespect to the O

+k frame.

18.6 Sensitivity of Articulated Body Quantities

Define the block-diagonal, symmetric operator λθias the partial derivative of λwith

respect to the ith generalized velocity coordinate, i.e.,

λθi

�=

∂λ

∂θi

(18.52)


Lemma 18.14 Sensitivity of articulated body quantities.λθi

is related to the Pθipartial derivative by the following expression:

λθi= Pθi

+P ˜Hω

≺i − ˜Hω

≺i P (18.53)

Also, the partial derivatives of various articulated body inertia operators are givenby the following expressions:

Dθi= Hλθi

H∗ (18.54a)

[D−1]θi

= −D−1HλθiH∗D−1 (18.54b)

[G]θi= τλθi

H∗D−1+ ˜H

ω

≺i G (18.54c)

[τ]θi= τλθi

H∗D−1H+ ˜Hω

≺i τ−τ ˜Hω

≺i (18.54d)

τθi= −[τ]θi

(18.54e)

[Eψ]θi= ˜H

ω

�i Eψ −Eψ˜H

ω

≺i

−Eφ

(

τλθiH∗D−1H− H

v=i τ

)

(18.54f)

λθi−Eψλθi

E∗ψ =

(

˜Hω

=i P−P ˜Hω

=i

)

+Eφ

(

Hv=i P+

−P+˜H

v

=i

)

E∗φ (18.54g)

Proof: Equation (18.53) follows from the use of (18.26) and (18.28) in the expres-sion for λ in (18.42). A similar process applied to the time derivative expressions in(18.43) and (18.44) leads to the sensitivity expressions in (18.54).

Remark 18.2 Link level sensitivities of articulated body inertias.Noting that

λθi(k) =

⎧⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎩

0 for k≺ i˜H∗

ω(i)P(i)−P(i) ˜H∗ω(i) for k = i

φ(k, i)(

τ(i)λθi(i)τ∗(i)+ H

∗v(i)P+(i)

−P+(i) ˜H∗v(i) (i)

)

φ∗(k, i) for k = ℘(i)

ψ(k,℘(i)) λθi(℘(i)) ψ∗(k,℘(i)) for k ℘(i)

(18.55)

it follows that, at the link level, the sensitivity expressions can be expressed as:

Pθi(k) =

{˜H∗

ω(i)P(k)−P(k) ˜H∗ω(i) for k� i

λθi(k) = ψ(k, i)Pθi

(i)ψ∗(k, i) for k i (18.56)

Dθi(k) =H(k)Pθi

(k)H∗(k) ·1[k�i] (18.57)

18.7 Sensitivity of Innovations Factors 375

Gθi(k) = ˜H∗

ω(i)G(k)1[k≺i]

+τ(k)Pθi(k)H∗

(k)D−1(k) ·1[k�i] (18.58)

τθi(k) = [ ˜H∗

ω(i)τ(k)−τ(k) ˜H∗ω(i) ] ·1[k≺i]

+τ(k)Pθi(k)H∗

(k)D−1(k)H(k) ·1[k�i] (18.59)

[ψ(k+ 1,k)]θi= [ ˜H∗

ω(i)ψ(k+ 1,k)−ψ(k+ 1,k) ˜H∗ω(i) ] ·1[k≺i]

+φ(k+ 1,k)(

H∗v(k) τ(k) ·1[k=i]

−τ(k)Pθi(k)H∗

(k)D−1(k)H(k) ·1[k�i]

)

(18.60)

The values of λθi(k) and Pθi

(k) across the links in the system is illustrated inFig. 18.1.

k = 1k = ik = n i>ki<k

Pθi(k) :

H∗ω(i)P(k)−P(k)H∗

ω(i)λθi(k)

λθi(k) :

0ψ(k,℘(i)) λθi(℘(i)) ψ∗(k,℘(i))

H∗ω(i)P(i)−P(i)H∗

ω(i)

Fig. 18.1 Expressions for of λθi(k) and Pθi

(k) for the links across the system

18.7 Sensitivity of Innovations Factors

Exercise 18.11 Time derivative of (I+HφK).Show that

d[I+HφK]

dt=Hφ

[

˜ΔωV φK+EφΔ

vVφG+τλH∗D−1

]

(18.61)


Exercise 18.12 Time derivative of [I+HφK]D.Show that

d[I+HφK]D

dt=Hφ

[

˜ΔωV φP+EφΔ

vVφP+ λ

]

H∗ (18.62)

Exercise 18.13 Time derivative of [I−HψK].Show that

d[I−HψK]

dt= −Hψ

[

˜ΔωV φK+EφΔ

vVφG

+τλH∗D−1]

[I−HψK]

(18.63)

Exercise 18.14 Sensitivities of [I+HφK] and [I−HψK].Show that

[I+HφK]θi=Hφ[ ˜H

ω

=i φP+Eφ˜H

v

=iφP+τλθi]H∗D−1 (18.64)

[I−HψK]θi= −Hψ

[

˜Hω

=iφK+Eφ˜H

v

=iφG

+τλθiH∗D−1

]

(I−HψK) (18.65)

The following exercise develops expressions for log {det {M}} and its partialderivatives with respect to generalized coordinates. This term appears in expressionsfor the Fixman potential [57,72] in the context of internal coordinate molecular dy-namics [88]. The Fixman potential is used in internal coordinate molecular dynam-ics simulations, to correct for systematic biases introduced in the computation ofstatistical thermodynamic quantities. Its partial derivatives with respect to the gen-eralized coordinates are the generalized forces resulting from the Fixman potential.

Exercise 18.15 Sensitivity of log {det {M}}.

1. Show that

∂ log {det {M}}

∂θi= Trace

{D−1Hλθi

H∗}

=∑

∀k: k�i

Trace{D−1

(k)H(k)λθi(k)H∗

(k)} (18.66)

2. For the case of systems with non-prismatic hinges (i.e., ΔV = 0), Jain [72] usesthe (A.29) matrix trace identity to alternatively establish that

18.7 Sensitivity of Innovations Factors 377

∂ log {det {M}}

∂θi= 2Trace {PΩHω

=i } (18.67)

Show that (18.67) and (18.66) are equivalent for systems with non-prismatichinges.

3. Also, show that the generalization of (18.67) for systems with prismatic hingesis:

∂ log {det {M}}

∂θi= 2Trace

{PΩ

(

Hω=i +Eφ H

v=i

)}

= 2Trace{P(i)Υ(i) ˜H∗

ω(i)}

+ 2Trace{PΩEφ H

v=i

}(18.68)

Chapter 19Diagonalized Lagrangian Dynamics

The multibody dynamics models we have studied have used hinge coordinates andvelocities, for the system-level generalized coordinate and velocity vectors. For asystem with N degrees of freedom, (4.26) describes the following Lagrangian formof the equations of motion for the system:

M(θ)θ+C(θ, θ) = T, C(θ, θ)4.27= Mθ−

12∂θ

∗Mθ

∂θ(19.1)

The mass matrix M is a configuration dependent, dense matrix, and the C termdepends non-linearly on both the configuration and velocity coordinates. In thischapter, we study alternative choices of generalized coordinates and velocities thatcan diagonalize and simplify the equations of motion [1, 2, 63, 84, 90, 168].

19.1 Globally Diagonalized Dynamics

The global diagonalization approach seeks to transform the configuration coordi-nates θ and their time-derivatives θ into a new set of (ϑ, ϑ) coordinates, in whichthe equations of motion are decoupled. This approach is based on the requirementthat the “square-root” factor of the mass matrix, m(θ), is the gradient of a globalcoordinate transformation.

Assumption 19.1 requires that the mass matrix factor m(θ) be the gradient ofsome function g(θ). This assumption is very rarely satisfied in practice [25, 105,171]. Nonetheless, it is of interest to examine the globally diagonalized equationsas a step towards the locally diagonalizing transformations discussed later in thischapter.


380 19 Diagonalized Lagrangian Dynamics

Assumption 19.1 (Global diagonalization condition). There ex-ists a smooth and invertible, global generalized coordinate transforma-tion ϑ = g(θ) ∈RN such that

∇θϑ= ∇θg=m∗(θ) ∈RN×N (19.2)

where the matrix function m(θ) is the “square-root” of the mass matrix,i.e.,

m(θ)m∗(θ) = M(θ) (19.3)

for all θ.

Assumption 19.1 requires that the mass matrix factor m(θ) be the gradient ofsome function g(θ). This assumption is very rarely satisfied in practice [25, 105,171]. Nonetheless, it is of interest to examine the globally diagonalized equationsas a step towards the locally diagonalizing transformations discussed later in thischapter.

Since g(θ) does not depend on θ, it follows that the new ϑ generalized velocityvector is

ϑ= ∇θg θ 19.2= m∗(θ)θ (19.4)

In terms of the velocity vector, ϑ, the kinetic energy is

Ke(ϑ, ϑ)4.25=

12θ∗Mθ

19.3=

12θ∗mm∗ θ 19.4

=12ϑ∗ϑ (19.5)

Lemma 19.1 Global diagonalizing coordinate transformations.When Assumption 19.1 holds, the equations of motion in the new coordinates (ϑ, ϑ)are

ϑ= ζ where ζ�= �(θ)T ∈RN and �(θ)

�= m−1(θ) (19.6)

Proof: Differentiating (19.4),

ϑ= m∗ θ+ m∗ θ ⇒ θ = �∗[ϑ− m∗ θ]

Use of this in (19.1) and pre-multiplication by � leads to

ϑ+C(ϑ, ϑ)= ζ where C(ϑ, ϑ)�= �C(θ, θ)− m∗ θ (19.7)

However,

∂θ∗Mθ

∂θ

19.5=

∂ϑ∗ϑ

∂θ= 2

∂ϑ∗

∂θϑ = 2

ddt∂ϑ∗

∂θϑ

19.2= 2mϑ (19.8)

Also,Mθ

19.3= (mm∗ +mm∗)θ 19.4

= mϑ+mm∗ θ (19.9)

19.1 Globally Diagonalized Dynamics 381

Thus,

C(ϑ, ϑ)19.1,19.7

= �

(

Mθ−12∂θ

∗Mθ

∂θ

)

− m∗ θ

19.8,19.9= �

(

mϑ+mm∗ θ− mϑ)

− m∗ θ 19.6= 0

Using this in (19.7) establishes the equations of motion in (19.6).The new equations of motion in (19.6) are very simple. The mass matrix is the

configuration independent identity matrix, and there are no Coriolis forces. Thecomponent degrees of freedom are completely decoupled and governed by indepen-dent second-order linear differential equations. Since T is the vector of generalizedforces corresponding to the generalized velocities vector θ, the principle of virtualwork implies that ζ is the vector of generalized forces corresponding to the general-ized velocities ϑ. Equation (19.6) can also be obtained alternatively by deriving theLagrangian equations of motion in the ϑ coordinate system using the diagonalizedexpression (19.5) for the kinetic energy in Exercise 4.11 on page 72.

Now that the simplicity resulting from the global coordinate transformation g(θ)is apparent, we examine conditions under which Assumption 19.1 is satisfied bymultibody systems. The answer is based on a result from non-Euclidean geometry.It is known that the mass matrix M defines a metric tensor [46] on the configurationmanifold. Since tensor quantities are invariant under coordinate transformations,a globally diagonalizing transformation exists if and only if the metric tensor isa Euclidean metric tensor, i.e., one with constant coefficients. A manifold with aEuclidean metric is said to be “flat” and the curvature tensor associated with it isidentically zero. The precise necessary conditions for the metric tensor associatedwith M to be a Euclidean metric are summarized in the following lemma.

Lemma 19.2 Euclidean metrics have zero curvature tensor.For Assumption 19.1 to hold, it is necessary that the curvature tensor R of M van-ish, that is, each of the N(N+ 1)/2 Riemannian symbols of the first kind Rhijkdefined below must vanish.

Rhijk�=

12

[

∂2Mhk

∂θi∂θj+∂2Mij

∂θh∂θk−∂2Mhj

∂θi∂θk−∂2Mik

∂θh∂θj

]

+∑l

[{lij

}[hk, l]−

{lik

}[hj, l]

] (19.10)

The [ij,k] and{kij

}quantities are the Christoffel symbols of the first and second

kind, respectively [119]. While the former are defined by (4.35) on page 68, thesecond kind are defined as

{kij

} �=

N∑m=1

M−1(k,m) [ij,m] (19.11)

Proof: See [25, 46, 171].


In practice, the conditions in this lemma are very restrictive, and are rarely satis-fied by multibody systems. Furthermore, they are extremely difficult to verify, sincethe first and second derivatives of the mass matrix with respect to the configurationvariables θ are required. Also, this condition must hold throughout the configura-tion space. The next section describes an alternative approach to diagonalizing theequations of motion that is broadly applicable to multibody systems.

19.2 Diagonalization in Velocity Space

Instead of diagonalizing globally in configuration space, we now explore diagonal-izing transformations in just velocity space. This transformation replaces the joint-angle velocities θ with a new set of velocities η, while retaining the original θ gen-eralized coordinates. The search for the generalized velocity transformation beginswith the following assumption regarding the factorization of the mass matrix.

Assumption 19.2 (Velocity space condition). There exists asmooth, differentiable and invertible function m(θ), with inverse denotedby �(θ), which factors the mass matrix as M(θ)= m(θ)m∗(θ) for allconfigurations. Unlike Assumption 19.1, m∗(θ) is not required to be thegradient of any function.

Under this assumption, the new generalized velocity coordinates η are defined as

η�= m∗(θ)θ (19.12)

The differentiability of m insures that the η vector is differentiable. Invertibility ofm(θ) insures that time derivatives θ of the configuration variables can be recoveredfrom η using the relationship:

θ19.6,19.12

= �∗(θ)η (19.13)

Under these conditions, η represents a valid choice for a new generalized velocityvector.

Assumption 19.2 is much weaker than Assumption 19.1. Since m is no longerthe gradient of a function, the transformed velocity vector η is no longer the timederivative of any vector of configuration variables. The η(k) components are thusquasi-velocities. Numerical integration of the η vector over time does not lead tothe generalized coordinates. The equations of motion using the new generalizedvelocity coordinates are described in the following lemma.

Lemma 19.3 Diagonalizing velocity transformations.When Assumption 19.2 holds, the equations of motion using the (θ,η) coordinatesare

η+C(θ,η) = ζ where ζ=�(θ)T (19.14)

19.2 Diagonalization in Velocity Space 383

with the Coriolis force vector

C(θ,η)�= �

(

mη−12∂θ

∗Mθ

∂θ

)

(19.15)

Proof: Differentiating (19.12),

η= m∗ θ+m∗θ ⇒ θ = �∗[η− m∗ θ]

Use of this in (19.1) and pre-multiplication by � leads to

η+C(θ,η)= ζ where C(θ,η) = �C(θ, θ)− m∗ θ (19.16)

Also,Mθ

19.3= (mm∗ +mm∗)θ 19.12

= mη+mm∗ θ (19.17)

Thus,

C(θ,η)19.1,19.16

= �

(

Mθ−12∂θ

∗Mθ

∂θ

)

− m∗ θ

19.17= �

(

mη+mm∗ θ−12∂θ

∗Mθ

∂θ

)

− m∗ θ

= �

(

mη−12∂θ

∗Mθ

∂θ

)

Using this in (19.16) establishes the result.These equations of motion are simpler than the original ones in (19.1) and are

similar to the globally diagonalized equations in (19.6). The mass matrix is onceagain constant and equal to the identity matrix. The main difference is that the Cori-olis force term is no longer zero. However, as we will see in the next section, thisCoriolis vector has the special property that it is orthogonal to the generalized ve-locity vector η, implying that the Coriolis term does no mechanical work.

The diagonal equations of motion in (19.14) occupy a middle ground between theglobally decoupled equations of motion in Lemma 19.1 and the standard equationsof motion in (19.1). While they are not quite as simple as the globally diagonalizedequations of Lemma 19.1, we will see later that they always exist for the broad classof tree-topology systems. This is in stark contrast to the rare existence of globallydiagonalizing transformations.

The m(θ) mass matrix factor plays a central role in the above diagonalized equa-tions. Numerical (e.g., Cholesky-like) factorization of the mass matrix at each con-figuration can be used to obtain a candidate factor m(θ). The resulting factors how-ever may not smoothly depend on the configuration coordinates and thus, might notbe differentiable. Even more problematically, numerical factorization proceduresprovide no systematic way to compute the Coriolis force term C(θ,η), since thederivatives of m are required for this purpose. An alternative path that overcomes


the limitations of the numerical factorization approach uses the SKO operator fac-torizations of the mass matrix and is discussed in Sect. 19.3.

19.2.1 Coriolis Force Does No Work

The following lemma shows that the Coriolis term C(θ,η) from the velocity spacediagonalization is orthogonal to the generalized velocities η and, therefore, does nomechanical work.

Lemma 19.4 C(θ,η) does no work.The Coriolis forces vector C(θ,η) does no work, i.e.,

η∗C(θ,η)= 0 (19.18)

Proof: Observe that M = mm∗ implies that θ∗Mθ θ= 2col

{η∗m∗

θiθ}

. Conse-

quently,

η∗C(θ,η) = θ∗(

mη− col{η∗m∗

θiθ

})

= θ∗mη−

N∑i=1

θ(i)η∗m∗θiθ

= θ∗mη−η∗m∗ θ = 0

The orthogonality of the nonlinear Coriolis forces is similar to the orthogonalityconditionω∗[ωJω] = 0 of the gyroscopic force term in the equations of motionfor a single rigid body rotating with angular velocityω. This property is in contrastto the non-orthogonality of the regular Coriolis forces term C(θ, θ) in the regularequations of motion in (19.1), for which θ

∗C(θ, θ)�= 0, as discussed in Exercise 4.4

on page 68.

19.2.2 Rate of Change of the Kinetic Energy

Recall that the kinetic energy of the system is Ke(θ,η)= 12η

∗η.

Lemma 19.5 The rate of change of the kinetic energy.Analogous to the Exercise 4.5 on page 68, the rate of change of the kinetic energyis the dot product of the generalized forces and generalized velocities

dKe(θ,η)dt

=η∗ζ (19.19)

19.3 The Innovations Factors as Diagonalizing Transformations 385

Proof:dKe(θ,η)

dt=η∗ η= η∗[ζ−C(θ,η)]

19.18= η∗ζ

19.3 The Innovations Factors as Diagonalizing Transformations

The Innovations factorization of the mass matrix in (9.49) provides candidate ve-locity diagonalizing transformations. Define the operators m(θ) and �(θ) as:

m(θ)�= [I+HAK]D

12 �(θ)

�= m−1(θ)

9.50= D− 1

2 [I−HψK] (19.20)

so that

M(θ)9.49= m(θ)m∗(θ) and M−1(θ)

9.51= �∗(θ)�(θ) (19.21)

The function m(θ) so defined satisfies all of the conditions in Assumption 19.2.Satisfaction of the differentiability property is based on the following rationale.The operators H and A are smooth and differentiable functions of the configura-tion coordinates, so the only potential trouble-spot is in the differentiability of thearticulated body quantities in (16.8), particularly the inverse D−1 of the diagonaloperator D =HPH∗. The diagonal matrix D is always positive definite, invertibleand a smooth function of the generalized coordinates. Consequently, D−1 is alwaysa smooth and differentiable function of θ.

One issue we have is that m depends upon D12 . Since D is block-diagonal, so

is D12 . In general, the square-root of a matrix is not unique for matrices of dimen-

sion greater than one. Thus, D12 is unique (modulo sign) only when the block ele-

ments are scalar, which occurs when the multibody system has only single degreeof freedom hinges. Uniqueness, and in fact smoothness, is required of D

12 for the

smoothness of m. Due to this constraint, for the rest of this section, we will assumethat all the hinges in the system are indeed single degree of freedom hinges. Thisrequirement is not much of a restriction since any multiple degree of freedom hingecan be expressed as a sequence of single degree of freedom hinges.

With the 1 degree of freedom hinge assumption, m = [I+HAK]D12 is a smooth

and differentiable matrix function. m(θ), therefore, satisfies all the conditions inAssumption 19.2 and is a valid velocity diagonalizing transformation. This conclu-sion is summarized in the following lemma.

Lemma 19.6 The SKO velocity diagonalizing transformation.Systems with an SKO model have velocity diagonalizing transformations satisfyingAssumption 19.2, with

m�= [I+HAK]D

12 (19.22)


and the diagonalized generalized velocity coordinates defined by

η= D12 [I+HAK]∗ θ and θ = [I−HψK]∗D− 1

2 η (19.23)

Proof: This lemma follows from (19.20) to (19.21) and the above discussion re-garding the smoothness of m.

19.3.1 Transformations Between θ and η

The η diagonalizing velocity coordinates are related to the θ generalized velocity co-ordinates via the transformation η= m∗ θ. This transformation can be mechanizedby an outward recursion from the base of the manipulator to its tip. This outwardrecursion is specified by the algorithm on the left column of Table 19.1. The inversetransformation θ = �∗η can also mechanized by an outward recursion. The rightcolumn of Table 19.1 shows this algorithm.

Similarly, the “new” input variables ζ, appearing in the diagonalized equationη+C(η,θ)= ζ, are obtained from the “old” inputs T by the transformation ζ= �T.This is mechanized by the inward, tip-to-base recursion specified on the left columnof Table 19.2. The inverse operation T = mζ, from the new variables ζ to the oldvariables T, is also performed recursively in an outward direction, as specified bythe algorithm in the right column of Table 19.2.

Table 19.1 θ and η can be computed recursively from each other

η= m∗θ= D12 [I+HAK]∗θ θ= �∗η= [I−HψK]∗D− 1

2η

V(N+1) = 0for k = N · · ·1

V+(k) = A∗(k+1,k)V(k+1)

η(k) = D12 (k)[θ(k)+G∗(k)V+(k)]

V(k) = V+(k)+H∗(k)θ(k)

end loop

V(N+1) = 0for k = N · · ·1

V+(k) = A∗(k+1,k)V(k+1)

θ(k) = D− 12 (k)η(k)−G∗(k)V+(k)

V(k) = V+(k)+H∗(k)θ(k)

end loop

It is relatively easy therefore, to go back and forth between the original θ and T

variables in traditional robot dynamics and the η and ζ variables in the diagonalizedequations of motion. The two mutually reciprocal outward recursions in Table 19.1govern the relationships between the new and old velocities. Similarly, the twomutually reciprocal inward recursions in the Table 19.2 govern the relationshipsbetween the T and ζ generalized forces. The term “mutually reciprocal” indicatesthat the corresponding spatial operations are mathematical inverses of each other.Each of the above four recursions represents an O(N) computational algorithm, in

19.4 Expression for C(θ,η) for Rigid-Link Systems 387

Table 19.2 ζ and T can be recursively computed from each other

ζ= �T = D− 12 [I−HψK]T T = mζ= [I+HAK]D

12 ζ

z(0) = 0for k = 1 · · ·N

z(k) = A(k,k−1)z+(k−1)

ζ(k) = D− 12 (k)[T(k)−H(k)z(k)]

z+(k) = z(k)+G(k)ζ(k)

end loop

z(0) = 0for k = 1 · · ·N

z(k) = A(k,k−1)z+(k−1)

T(k) = D12 (k)ζ(k)+H(k)z(k)

z+(k) = z(k)+G(k)ζ(k)

end loop

the sense that the number of required arithmetical operations increases only linearlywith the number of degrees of freedom. Generalization of the serial-chain recursionsin Tables 19.1 and 19.2 to tree-topology systems simply requires the replacement ofthe base-to-tip recursions by base-to-tips scatter recursions, and the tip-to-base re-cursions by tips-to-base gather recursions.

19.4 Expression for C(θ,η) for Rigid-Link Systems

The C(θ,η)= �(mη− 12∂θ

∗Mθ∂θ

) Coriolis term in (19.15) is one of the key parts ofthe diagonalized equations of motion η+C(θ,η)=ζ. The following sections deriveanalytical operator expressions for the component terms as a step towards obtainingan operator expression for C(θ,η). The expressions for the time derivatives andsensitivities of the spatial operators are required. These expressions for rigid-linkSKO models have been derived in Chap. 18. In order to utilize them, we narrowour attention to rigid-link systems for the remainder of this chapter. We signifythis change by switching from the more general EA and A operators to Eφ and φrigid-link system operators.

19.4.1 Closed-Form Expression for mη

In this section, we derive explicit analytical expressions for mη needed by C(θ,η).

Exercise 19.1 Time derivative of D12 .

Show that for a system with 1 degree of freedom joints,

dD12

dt=

12D− 1

2HλH∗ (19.24)


Lemma 19.7 Expression for m.For a system with 1 degree of freedom joints, the time derivative of the mass matrixfactor m is given by:

m =Hφ

[

˜ΔωV φK+EφΔv

VφG+12(I+τ)λH∗D−1

]

D12

=Hφ

[

˜ΔωV φP+EφΔv

VφP+12(I+τ)λ

]

H∗D− 12

(19.25)

where λ is as defined in Section 18.5.Proof: We have

m19.20=

d[I+HφK]

dtD

12 +[I+HφK]

dD12

dt18.61,19.24

= Hφ[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

D12 +

12HφGD− 1

2HλH∗

=Hφ[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

D12 +

12HφGHλH∗D− 1

2

=Hφ[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

D12 +

12HφτλH∗D− 1

2

=Hφ

[

˜ΔωV φK+EφΔv


]

D12

This establishes the first equality in (19.25). The latter equality is simply a rear-rangement of the first one. We have used the D− 1

2HλH∗ = HλH∗D− 12 identity,

which holds for systems with 1 degree of freedom joints, when going from step 2 to3 in the sequence above.

Lemma 19.8 Expression for mη.For a system with 1 degree of freedom hinges,

mη=Hφ[(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗

+12

(

˜ΔωV P−P ˜ΔωV +Eψλ− λE∗ψ+Eφ

{Δv

V P+ −P+˜ΔvV

}E∗φ

)]

V

(19.26)

Proof:

mη19.25= Hφ

[

˜ΔωV φP+EφΔv

VφP+12(I+τ)λ

]

H∗D− 12 η

19.23= Hφ

[

˜ΔωV φP+EφΔv

VφP+12(I+τ)λ

]

H∗[I+HφK]∗ θ (19.27)


Now

[I+HφK]∗ θ (9.38)= H∗(G∗φ∗H∗)θ (9.1)

= H∗G∗V = τ∗V (19.28)

Using (19.28) in (19.27), we have

mη19.28= Hφ

[

(

˜ΔωV Eφ+EφΔv

V

)

φP+12(I+τ)λ

]

τ∗V

(13.60)= Hφ

[

(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗ +12(I+τ)λτ∗

]

V

(9.40)= Hφ

[

(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗ +12(I+Eψ)λ(I−E∗

ψ)

]

V

=Hφ

[

(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗ +12(λ−EψλE

∗ψ+Eψλ− λE∗

ψ)

]

V

(18.44)= Hφ

[(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗

+12

(

˜ΔωV P−P ˜ΔωV +Eφ

(

Δv

V P+ −P+˜ΔvV

)

E∗φ+Eψλ− λE∗

ψ

)]

V

19.4.2 Operator Expression for C(θ,η)

Lemma 19.9 Operator expression for C(θ,η).For a system with 1 degree of freedom hinges,

C(θ,η) = D− 12Hψ

[

VM−12

(

˜ΔωV P+P ˜ΔωV +Eφ

{Δv

V P+ +P+˜ΔvV

}E∗φ

+Eψλ− λE∗ψ

)]

V

(19.29)

Proof: From (19.15) it follows that:


C(θ,η)18.38c,19.26

= �Hφ[

VM−(

EφΔv

V + ˜ΔωV

)

[I+φKH]P

+(

˜ΔωV Eφ+EφΔv

V

)

φPτ∗ +12

(

˜ΔωV P−P ˜ΔωV

+Eφ

{Δv

V P+ −P+˜ΔvV

}E∗φ+Eψλ− λE∗

ψ

)]

V

9.45,9.38= D− 1

2Hψ

[

VM−EφΔv

V [I+φKH]P− ˜ΔωV P

+EφΔv

VφPτ∗ +12

(

˜ΔωV P−P ˜ΔωV

+Eφ

{Δv

V P+ −P+˜ΔvV


ψ

)]

V

= D− 12Hψ

[

VM+EφΔv

V

{φτ−[I+φKH]

}P

+12

(

− ˜ΔωV P−P ˜ΔωV +Eφ{Δv

V P+ −P+˜ΔvV

}E∗φ

+Eψλ− λE∗ψ

)]

V

(19.30)Now

φτ−[I+φKH]9.45= φτ−φψ−1 = φ

(

τ−(I−Eψ))

= φ(

−τ+Eφτ)

= −φφ−1τ= −τ (19.31)

Using (19.31) in (19.30) leads to

C(θ,η) = D− 12Hψ

[

VM−EφΔv

V P+ +12

(

− ˜ΔωV P−P ˜ΔωV

+Eφ

{Δv

V P+ −P+˜ΔvV


ψ

)]

V

= D− 12Hψ

[

VM−12

(

˜ΔωV P+P ˜ΔωV

+Eφ

{Δv

V P+ +P+˜ΔvV


ψ

)]

V

Lemma 19.9 explicitly derives, in terms of relatively simple quantities, the verycomplicated quantity C(θ,η) = �(m− 1

2 θ∗Mθ θ), which depends on various deriva-

tives of the system mass matrix. Using this expression, Algorithm 19.1 describesa recursive procedure for computing C(θ,η). This algorithm is a recursive imple-mentation of (19.29). It proceeds from tip-to-base and is of O(N) computationalcomplexity. It assumes that the spatial velocities V have already been computed us-ing Table 19.1, for example. Similarly, if the articulated body inertia quantities arenot available then they can be computed concurrently.


Algorithm 19.1 Computation of the C(θ,η) Coriolis forces vector

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

λ(0) = 0, y(0) = 0

for k = 1 · · ·nX(k) = ˜ΔωV (k)P(k)

Z(k) = φ(k,k− 1)Δv

V (k− 1)P+(k− 1)φ∗(k,k− 1)

S(k) = X(k)−Z(k)

Y(k,k− 1) =ψ(k,k− 1)λ(k− 1)

λ(k) = Y(k,k− 1)ψ∗(k,k− 1)+S(k)+S∗(k)

y(k) =∑i∈�(k)

ψ((k), i)y(i)−[

−2V(k)M(k)+X(k)−X∗(k)]

V(k)

+Y(k,k− 1)V(k− 1)− λ(k)τ∗(k)V+(k)

C(k) =12D− 1

2 (k)H(k)y(k)

end loop

Exercise 19.2 Non-working C(θ,η) Coriolis Vector.Derive the orthogonality result η∗C(θ,η) = 0 in (19.18) starting with the analyticalexpression for C(θ,η) in (19.29).

Remark 19.1 Coriolis expressions for systems with revolute hinges.For systems with only revolute hinges, ΔvV = 0, and, hence, the following simplerversions of (18.38), (19.26), and (19.29) hold for such systems:

C(θ,η) =12D− 1

2Hψ

[

Eψλ− λE∗ψ− ˜ΔωV P−P ˜ΔωV + 2VM

]

V

∂θ∗Mθ

∂θ= −2Hφ

{V− ˜ΔωV φ

}MV

mη=Hφ[

˜ΔωV φKHP+12

(

˜ΔωV P−P ˜ΔωV +Eψλ− λE∗ψ

)]

V

(19.32)

Exercise 19.3 Expression for �.For a system with 1 degree of freedom joints, show that the time derivative of the �

mass matrix inverse factor is given by:


� = −D− 12Hψ

[

˜ΔωV φK+EφΔv


]

[I−HψK] (19.33)

Exercise 19.4 Sensitivity of Innovations factors.For a system with 1 degree of freedom joints, show that

mθi =Hφ[ ˜Hω

=i φP+Eφ ˜Hv

=iφP+12(I+τ)λθi ]H

∗D− 12 (19.34)

�θi = −D− 12Hψ

[

˜Hω

=iφKD+Eφ ˜Hv

=iφG+12(I+τ)λθiH

∗]

D− 12 � (19.35)

19.4.3 Decoupled Control

The diagonal equations can also be used to design controllers that are decoupled andnon-interacting. The decoupled control approach exploits the dynamical behavior ofthe η coordinates.

Though control objectives are defined in the physical (θ,T) domain, the controlsystem is designed in the (η,ζ) variables for the diagonalized equations of motion.Stability in the η, ζ coordinates is equivalent to stability in the original θ, θ coordi-nates. The control problem consists of finding a feedback relationship for the ζ inputin terms of the η velocities. Once ζ is determined, it is possible to go back to phys-ical space to determine the required T physical inputs by means of the relationshipT = mζ, using the inwardly recursive algorithm in Table 19.2.

Lemma 19.10 Rate feedback control using diagonalized dynamics.The rate feedback control

ζ= −cη,

in which c is a positive diagonal control gain matrix, renders the system stable inthe sense of Lyapunov.

Proof: This result follows by using the kinetic energy as a Lyapunov function andobserving that its time derivative, in Lemma 19.5, can be guaranteed to be negativedefinite, by the choice of the above control law.

This control algorithm involves rate feedback only. It can be referred to as a“rate” control algorithm because the feedback quantity is a velocity, in fact, it isa vector of the diagonalizing generalized velocities. It does not guarantee that themanipulator will end up in a prescribed configuration. Additional applications of thediagonalized formulation for manipulator control can be found in [3, 84, 106].

19.5 Un-normalized Diagonalized Equations of Motion 393

19.5 Un-normalized Diagonalized Equations of Motion

An alternative set of diagonalized equations of motion can be obtained by using aslightly different generalized velocity vector defined as

ξ�= [I+HφK]∗ θ and θ

9.50= [I−HψK]∗ξ (19.36)

The kinetic energy in these coordinates is

Ke(ξ, ξ)=12ξ∗D(θ)ξ (19.37)

While no longer the constant identity matrix, the mass matrix for this formulationis the block-diagonal D(θ) matrix. While configuration dependent, this mass ma-trix retains the simpler block-diagonal structure. Also, in this new formulation, theassumption of 1 degree of freedom hinges is no longer required. The equationsof motion in the new (θ,ξ) coordinates are described in the following lemma.

Lemma 19.11 Equations of motion with un-normalized diagonalizedcoordinates.

Dξ+C(θ,ξ) = κin where κin�= [I−HψK]T (19.38)

C(θ,ξ)�=Hψ

[

− ˜ΔωV P−EφΔv

V P+ + λτ∗ + VM]

V

19.28= Hψ

[

λH∗ξ−(

˜ΔωV P+EφΔv

V P+ − VM)

V]

(19.39)

Proof: Using steps paralleling the derivation of (19.15), we have

C(θ,ξ) = [I−HψK]

(

d[I+HφK]D

dtξ−

12∂θ

∗Mθ

∂θ

)

18.62,18.38c,9.45= Hψ

([

˜ΔωV φP+EφΔv

VφP+ λ]

H∗ξ

+{

VM−(

EφΔv

V + ˜ΔωV

)

[I+φKH]P}

V)

19.28= Hψ

([

˜ΔωV φP+EφΔv

VφP+ λ]

τ∗

+{

VM−(

EφΔv

V + ˜ΔωV

)

[I+φKH]P})

V

=Hψ({[

˜ΔωV +EφΔv

V

]

(

φτ−[I+φKH])

− ˜ΔωV τ}

P

+ λτ∗ + VM)

V

19.31= Hψ

({−

[

˜ΔωV +EφΔv

V

]

τ− ˜ΔωV τ

}P+ λτ∗ + VM

)

V

=Hψ({

− ˜ΔωV −EφΔv

V τ

}P+ λτ∗ + VM

)

V


While these equations of motion are still diagonal, they differ from those in(19.14) in two respects. First, although the mass matrix D is diagonal, it is config-uration dependent. Moreover, while the Coriolis forces term C(θ,ξ) is simpler thanC(θ,η), it is no longer orthogonal to the generalized velocities vector. Table 19.3compares the properties of the regular and the different diagonalizing formulationswe have studied so far.

Table 19.3 Comparison of the properties of the regular and different diagonalizing formulationsfor tree-topology system dynamics

Regular Un-normalized Normalized Global

Section 8.7 19.5 19.2 19.1

Coordinates (θ, θ) (θ,ξ) (θ,η) (ϑ, ϑ)

Diagonal M√ √ √

Constant M√ √

Non-working C√ √

C = 0√

An O(N) computational algorithm for the components of C(θ,ξ) is shown inAlgorithm 19.2.

Algorithm 19.2 Computation of the C(θ,ξ) Coriolis forces vector⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

λ(0) = 0, y(0) = 0

for k = 1 · · ·NX(k) = ˜ΔωV (k)P(k)

λ(k) =ψ(k,k− 1)λ(k− 1)ψ∗(k,k− 1)+X(k)+X∗(k)

S(k) = X(k)+φ(k,k− 1)Δv

V (k− 1)P+(k− 1)− V(k)M(k)

y(k) =ψ(k,k− 1)y(k− 1)+λ(k)H∗(k)ξ(k)−S(k)V(k)

C(k) =H(k)y(k)

end loop

19.5.1 O(N) Forward Dynamics

One important application is that of forward dynamics and numerical integration topredict the motion of the manipulator in response to applied moments. An algorithmbased upon the un-normalized diagonalized equations of motion in Lemma 19.11 isdescribed here. The acceleration term is:

19.5 Un-normalized Diagonalized Equations of Motion 395

ξ19.38= D−1[κin−C(θ,ξ)

] 19.38= D−1 {[

I−HψK]

T −C(θ,ξ)}

= D−1 {T −HψKT −C(θ,ξ)}19.11= D−1 {T −Hψγ} (19.40)

whereγ

�= KT + λH∗ξ−

(

˜ΔωV P+EφΔv

V P+ − VM)

V (19.41)

The very first time, V must be computed explicitly (from θ or ξ) using one of thealgorithms in Table 19.1. Algorithm 19.3 is similar to the usual AB forward dynam-ics algorithm. However, it is a significant improvement because it is only a 2-sweepalgorithm involving an inward recursion to compute ξ followed by an outward re-cursion to compute θ. The Coriolis effects are completely accounted for in the singleinward sweep. The standard O(N) articulated body inertia algorithm typically re-quires at least one or two preliminary inverse dynamics sweeps, prior to utilizationof the forward dynamics algorithm.

Algorithm 19.3 Forward dynamics for un-normalized diagonalized dynamics1. Compute the articulated body inertia P and the shifted Kalman gain K using

the Riccati equation in (9.33). Compute also the corresponding time derivative λusing Algorithm 18.1 on page 372.

2. Compute the ξ generalized accelerations using the following tip-to-base recur-sive sweep:

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

μ(0) = 0

for k = 1 · · ·Nγ(k) = K(k,k− 1)T(k− 1)+ λ(k)H∗(k)ξ(k)

−[

˜ΔωV (k)P(k)− V(k)M(k)]

V(k)

μ(k) =ψ(k,k− 1)μ(k− 1)+γ(k)

ξ(k) = D−1(k)[

T(k)−H(k)μ(k)]

end loop

(19.42)

3. Conduct a numerical integration step to obtain the total joint rates ξ at a new timeinstant.

4. Compute the θ generalized velocities and the V spatial velocity by the outwardrecursion in Table 19.1, modified to account for the η= D− 1

2 ξ relationship.5. Integrate the θ generalized velocities to obtain the θ generalized coordinates at

the new time instant.6. Go back to the first step and repeat as long as necessary until a prescribed final

time has been reached.

Appendix AUseful Mathematical Identities

A.1 3-Vector Cross-Product Identities

The following useful cross-product identities hold for arbitrary 3-vectorsp, q, and r:

pp= 0

p∗ = − p

pq= − qp

( pq) = p q− q p

p q= qp∗ −p∗qIp qr+ r pq+ q rp= 0

p q qp= − q p pq

p∗ qr= r∗ pq= q∗ rp˜Rp= R pR∗

(A.1)

R denotes a rotation matrix.

A.2 Matrix and Vector Norms

The sup norm (or 2-norm) of a matrix A is defined as

‖A‖ �= sup

x‖Ax‖/‖x‖ (A.2)

where the norm of a vector x is defined as ‖x‖ �=

√x∗x.

Exercise A.1 Norm of s.Show that ‖s‖ = ‖s‖.

A. Jain, Robot and Multibody Dynamics: Analysis and Algorithms, 397DOI 10.1007/978-1-4419-7267-5, c© Springer Science+Business Media, LLC 2011

398 A Useful Mathematical Identities

A.3 Schur Complement and Matrix Inverse Identities

Assume that we have a block-partitioned matrix,

(

A B

C D

)

, with square A and D

sub-matrices. WhenD is invertible, the Schur complement of the matrix is definedas A−BD−1C. The following describe factorizations and solutions of equationsinvolving such block partitioned matrices.

1. IfD is invertible, then(

A B

C D

)

=

(

I BD−1

0 I

)(

A−BD−1C 0

0 D

)(

I 0

D−1C I

)

(A.3)

Hence,

(

A B

C D

)−1

=

(

I 0

−D−1C I

)(

(A−BD−1C)−1 0

0 D−1

)(

I −BD−1

0 I

)

(A.4)

Using this factorization, the solution to the following matrix equation(

A B

C D

)[

x1

x2

]

=

[

b1

b2

]

(A.5)

is given byy=D−1b2

x1 = (A−BD−1C)−1 (b1 −By)

x2 = y−D−1Cx1

(A.6)

2. If A is invertible, then,(

A B

C D

)

=

(

I 0

CA−1 I

)(

A 0

0 D−CA−1B

)(

I A−1B

0 I

)

(A.7)

Hence,

(

A B

C D

)−1

=

(

I −A−1B

0 I

)(

A−1 0

0 (D−CA−1B)−1

)(

I 0

−CA−1 I

)

(A.8)Using this factorization, the solution to the following matrix equation

(

A B

C D

)[

x1

x2

]

=

[

b1

b2

]

(A.9)

A.3 Schur Complement and Matrix Inverse Identities 399

is given by

y = A−1b1

x2 = (D−CA−1B)−1

(b2 −Cy)

x1 = y−A−1Bx2 (A.10)

3. If A, B and C are invertible, andD= 0, then (A.8) simplifies to:

(

A B

C 0

)−1

=

(

0 C−1

B−1 −B−1AC−1

)

(A.11)

4. Now let us assume that D is invertible, and further that there are constraintsQb2 = 0 and x2 = Q∗λ for some full-rank matrix Q and vector λ. Then (A.9)can be re-expressed as:

(

I 0

0 Q

)(

A B

C D

)(

I 0

0 Q∗

)[

x1

λ

]

=

[

b1

0

]

(A.12)

That is,

(

A BQ∗

QC QDQ∗

)[

x1

λ

]

=

[

b1

0

]

=⇒ (

A−BQ∗(QDQ∗

)−1QC

)

x1A.6= b1

(A.13)

QDQ∗ is invertible because D is invertible andQ is full-rank.5. Let us assume the same conditions as in (4), except that the original block-matrix

has an additional [c1, c2]∗ term to take the form:

(

A B

C D

)[

x1

x2

]

+

[

c1

c2

]

=

[

b1

b2

]

(A.14)

In this case, (A.13) generalizes to:

(

A BQ∗

QC QDQ∗

)[

x1

λ

]

=

[

b1 − c1

−Qc2

]

(A.15)

=⇒ (A−ZC)x1 +(c1 −Zc2)A.6= b1 where Z

�= BQ∗

(QDQ∗)−1Q

with λ = −(QDQ∗)−1Q(c2 +Cx1) and x2 =Q∗λ

6. Let us assume the same conditions as in (5), except that now the constraint hasthe form x2 =Q∗λ+γ. In this case, (A.13) generalizes to:


(

A BQ∗

QC QDQ∗

)[

x1

λ

]

=

[

b1 −(c1 +Bγ)

−Q(c2 +Dγ)

]

(A.16)

=⇒ (A−ZC)x1 +(

c1 +Bγ−Z(c2 +Dγ)) A.6

= b1

where Z�= BQ∗

(QDQ∗)−1Q

with λ = −(QDQ∗)−1Q(c2 +Dγ+Cx1) and x2 =Q∗λ+γ

A.4 Matrix Inversion Identities

1. For any matrix A such that (I−A) is invertible, the following matrix identityholds:

A(I−A)−1

= (I−A)−1A = (I−A)

−1− I (A.17)

2. For a pair of matrices A and B such that (I+AB) is invertible, we have

(I+AB)−1

= I−A(I+BA)−1B (A.18)

3. For matricesA,X, R and Y, such thatA, R and [A+XRY∗] are invertible, we have

[A+XRY∗]−1=A−1

−A−1X[

R−1+Y∗A−1X

]−1Y∗A−1 (A.19)

Lemma A.1 The 1-resolvent of a nilpotent matrix.

If U is a nilpotent matrix, such thatUn = 0, then its 1-resolvent,W�= (I−U)−1,

is given byW = I+U+U2

+ · · · +Un−1 (A.20)

Proof: ForW as defined in (A.20)

UW =WU=U+U2+ · · · +Un =U+U2

+ · · · +Un−1=W− I

Rearranging terms, we have

I =W−UW = (I−U)W =⇒ (I−U)−1

=W

A.5 Matrix Trace Identities

For a square matrix ATrace {A} = −Trace {−A} (A.21)

A.6 Derivative and Gradient Identities 401

For matrices A and B such that AB is square, we have

Trace {AB} = Trace {BA} (A.22)

A.6 Derivative and Gradient Identities

A.6.1 Function Derivatives

The following identities hold for function derivatives of a smooth function g(θ, θ):

∂g

∂θ=∂g

∂θ∂g

∂θ=

ddt∂g

∂θ+∂g

∂θ

∂g

∂θ=

ddt∂g

∂θ(A.23)

A.6.2 Vector Gradients

We now define the notation for gradients and derivatives of vector functions withrespect to a vector. Let f(θ) ∈Rm be a smooth function of a vector, θ ∈Rn. Thenthe gradient, ∇θf(θ), is anm×nmatrix defined as follows:

∇θf(θ) �=

⎛

⎜

⎜

⎝

∂f1∂θ1

· · · ∂f1∂θn

.... . .

...∂fm∂θ1

· · · ∂fm∂θn

⎞

⎟

⎟

⎠

∈Rm×n (A.24)

Exercise A.2 Product gradient and chain rules.

1. Given a vector x ∈ Rp, a smooth, vector-valued function f(x) ∈ Rm, and asmooth, scalar valued function g(x), show that the following product rule holds:

∇x[

f(x)g(x)]

= ∇xf(x) ·g(x)+ f(x) ·∇xg(x) (A.25)

2. Given a smooth vector-valued function f(y) ∈ Rm of another vector-valuedfunction y(x) ∈ Rn, which in turn is a smooth function of a vector x ∈ Rp,show that the following chain rule holds:

∇xf(x) = ∇yf(y) ·∇xy(x) (A.26)


At times, we need to differentiate a row-vector valued functions f(θ) with respectto the variables vector θ ∈Rm. We use the following notation for this purpose:

df(θ)dθ

�=

[∇θ(f∗)]∗

=

⎡

⎢

⎢

⎣

∂f∂θ1

...∂f∂θm

⎤

⎥

⎥

⎦

(A.27)

The notational convention in (A.27) also applies to partial derivatives such as ∂f∂θ

.

A.6.3 Matrix Derivatives

Let A(t) be a differentiable and invertible matrix. We derive an expression for thetime derivative of the inverse of A(t) in terms of the time derivative of A(t). SinceA(t)A−1(t) = I, differentiating both sides with respect to t yields

dA(t)

dtA−1

(t)+A(t)dA−1(t)

dt= 0

Rearranging terms, we obtain

dA−1(t)

dt= −A−1

(t)dA(t)

dtA−1

(t) (A.28)

Moreover, we have the following identity from Graham [61]:

d log {det {A(t)}}

dt= Trace

{A−∗ dA(t)

dt

}(A.29)

Appendix BAttitude Representations

This appendix summarizes some attitude representation schemes that are alter-natives to the direction-cosine matrix attitude representation scheme. References[43, 165] contains a detailed discussion on this topic. Since different conventionsexist, a key to working with attitude representations is a clear definition of how theycompose with each other, and how they transform vector representations. The IRB

direction-cosine matrix representations for attitude is the least ambiguous, becauseits composition operation is simply the normal matrix/vector multiplication opera-tion, i.e.,

Ip=IRB

Bp

for a 3-vector p. We refer to direction-cosine matrices as rotation matrices.

Exercise B.1 Time derivative of a rotation matrix.Let R(t) denote a rotation matrix that is a smooth function of time. Show that

dIRB

dt= w I

RB

for some 3-vector w. As discussed in (1.8) on page 5, w = Iω(I,B), the angularvelocity of the B frame with respect to the I frame expressed in the I frame.

In this appendix, we work exclusively with the I and B pair of frames whilestudying the properties of attitude representations. With this in mind, we use themore compactω notation forω(I,B) to simplify the expressions.

B.1 Euler Angles

Euler angles, θ= (ψ,γ,φ) are an example of a minimal, 3-parameter attitude repre-sentation. This representation is based on expressing rotations as consisting of a se-quence of three principal axis rotations. There are several possible options for Euler

403

404 B Attitude Representations

angle representations based on the specific choice of the principal axes [43,69]. Forinstance, the ZXZ Euler angles representation is defined as a sequence of principalaxis rotations about the z axis, the x axis, and, once again, the z axis. The expressionfor IRB for the ZXZ Euler angle representation is as follows:

IRB(θ) =

⎛

⎜

⎝

cψ −sψ 0

sψ cψ 0

0 0 1

⎞

⎟

⎠

⎛

⎜

⎝

1 0 0

0 cγ −sγ

0 sγ cγ

⎞

⎟

⎠

⎛

⎜

⎝

cφ −sφ 0

sφ cφ 0

0 0 1

⎞

⎟

⎠

=

⎛

⎜

⎝

cψcφ− sψcγsφ −cψsφ− sψcγcφ sψsγ

sψcφcψcγsφ −sψsφ+ cψcγcφ −cψsγ

sγsφ sγcφ 1

⎞

⎟

⎠(B.1)

The orientation of frame B is obtained through a sequence of rotations. We beginwith B aligned with frame I. Then, we rotate B about its z axis through the angleψ.Next, we rotate it about its x axis by the angle γ, and, finally, about its z axis by theangle φ, to obtain the final orientation of frame B.

For the ZXZ Euler angle representation, the Bω angular velocity is related to thetime derivatives of the Euler angles, θ, as follows:

Bω=

⎛

⎜

⎝

sγsφ cφ 0

sγcφ −sφ 0

cγ 0 1

⎞

⎟

⎠θ (B.2)

When sin(γ) �= 0, the matrix in (B.2) is invertible, and the inverse relationship isgiven by

θ=1sγ

⎛

⎜

⎝

sφ cφ 0

sγcφ −sγsφ 0

−cγsφ −cγcφ sγ

⎞

⎟

⎠

Bω (B.3)

The relationship in (B.3) is undefined when sin(γ) = 0. All 3-parameter attitude rep-resentations have such singularities. A representation of size 4 or more is requiredto avoid singularities.

B.2 Angle/Axis Parameters

Euler’s theorem states that every rotation is equivalent to a rotation about a fixedvector. With n denoting the axis of rotation unit vector, and θ the angle of rotation,the following exponential formula defines the expression for the rotation matrix:

IRB(n,θ) = exp[nθ] (B.4)

B.2 Angle/Axis Parameters 405

Thus, the four parameters, IρB

�= (n,θ), can be used to represent the relative

attitude of the two frames. This representation is known as the angle/axis repre-sentation of attitude.

Exercise B.2 Derivation of the Euler–Rodrigues formula.

1. Verify that the characteristic polynomial1 of a 3×3 skew-symmetric matrix s is

λ3+σ2λ (B.5)

where σ�= ‖s‖ is the vector norm of s.

2. Use this to derive the following Euler–Rodrigues formula for a rotation matrixin terms of its angle/axis representation coordinates:

IRB

(

IρB

)

= cos(θ)I3 +[

1 − cos(θ)]

nn∗+ sin(θ) ñ

= I3 +[

1 − cos(θ)]

ñ ñ+ sin(θ) ñ (B.6)

Exercise B.3 Trace and characteristic polynomial of a rotation matrix.Let R(n,θ) denote a rotation matrix.

1. Show thatγ

�= Trace {R(n,θ)} = 1 + 2cos(θ) (B.7)

2. Show that the characteristic polynomial of R(n,θ) is

λ3−γλ2

+γλ− 1 (B.8)

3. Verify that n is the eigen-vector of R(n,θ) with eigen-value of 1.4. Show that when sin(θ) �= 0, n can be obtained from the R(n,θ) rotation matrix

using the following relationship:

ñ = (R−R∗)/(2sin(θ)) (B.9)

Remark B.1 Eigen-values of a rotation matrix.The axis of rotation n for a rotation matrix IRB is its eigen-vector corresponding

to the eigenvalue 1. The angle of rotation, θ, appears in the other complex eigen-values of the matrix, which, using (B.8) can be shown to be of the form exp[jθ] and

1 The characteristic polynomial of a matrix, A, is defined as the polynomial of λ, defined by thematrix determinant, det(λI−A).


exp[−jθ]. Given a rotation matrix R instead of these relationships, it is simpler toobtain θ by using the (B.7) expression for the trace of a R and n from the expressionin (B.9).

Exercise B.4 Angular velocity from the angle/axis rates.Derive the following expression for the Bω angular velocity in terms of n and θ

angle/axis rates:

Bω = θn−(1 − cosθ)ñn+ sin(θ)n

=[

sin(θ)− (1 − cosθ)ñ, n]

[

n

θ

]

(B.10)

Exercise B.5 Angle/axis rates from the angular velocity.Show that the expression for the n and θ time derivatives in terms of the Bω an-

gular velocity is:[

n

θ

]

=

[

12

[

ñ− cot(θ/2)ññ]

n∗

]

Bω (B.11)

B.3 Unit Quaternions/Euler Parameters

Unit quaternions (also known as Euler parameters) are closely related to the an-gle/axis parameters and also consist of four scalar parameters. Given an angle/axisrepresentation of the attitude IρB = (n,θ), the corresponding quaternion represen-

tation, denoted IqB

�=

[

q

q0

]

∈R4, is defined as:

q0�= cos(θ/2); q

�=

⎡

⎢

⎣

q1

q2

q3

⎤

⎥

⎦= sin(θ/2)n (B.12)

It is easy to verify that quaternions have unit magnitude, that is,

q∗q = q20 +q∗q= 1 (B.13)

A description of quaternions and their transformations can be found in [30, 43, 69].

Exercise B.6 Quaternion expression for a rotation matrix.Show that the IRB rotation matrix can be obtained from the corresponding Iq

B=

(q,q0) quaternion representation by any of the following expressions:

B.3 Unit Quaternions/Euler Parameters 407

IRB

(

IqB

)

= (q20 −q∗q)I3 + 2qq∗ + 2q0 q

= (2q20 − 1)I3 + 2qq∗ + 2q0 q

= I3 + 2 q q+ 2q0 q

= I3 + 2(q0I3 + q) q

= (q0I3 + q)2+qq∗

=

⎛

⎜

⎝

2[q20 +q2

1]− 1 2[q1q2 −q0q3] 2[q1q3 +q0q2]

2[q1q2 +q0q3] 2[q20 +q2

2]− 1 2[q2q3 −q0q1]

2[q1q3 −q0q2] 2[q2q3 +q0q1] 2[q20 +q2

3]− 1

⎞

⎟

⎠

(B.14)

Observe that, in contrast to (B.6), the (B.14) expression for IRB(IqB) does not

involve trigonometric quantities. Indeed, one of the key advantages of quaternionsis that quaternion transformations typically involve algebraic expressions insteadof the computationally expensive trigonometric functions typically encountered forother types of attitude representations.

Exercise B.7 Basic properties of unit quaternions.Let q = (q,q0) denote a unit quaternion:

1. Show that the orthogonality of the matrix R(q) in (B.14) follows from the unitmagnitude of q.

2. Verify that the unit quaternions q and −q are equivalent, in the sense that theyboth map to the same rotation matrix, i.e. R(q) = R(−q).

3. Show that the inverse of a quaternion q, denoted q−1, is the unit quaterniongiven by

q−1=

[

−q

q0

]

(B.15)

An equivalent representation for q−1 is (q,−q0).4. Show that the trace of the R(q) rotation matrix is given by

Trace{R(q)

}= 4q2

0 − 1 (B.16)

5. Show that q is the eigen-vector of R(q) with eigenvalue of 1, i.e.,

Rq= q (B.17)

6. Show that when q0 �= 0, q can be obtained from the R rotation matrix using thefollowing relationship:

q= (R−R∗)/(4q0) (B.18)


B.3.1 The E+(q) and E−(q) Matrices

For a unit quaternion q = (q,q0), define the 4×4 matrices, E+(q) and E−(q), as[30, 69, 97]

E+(q)�=

(

q0I3 + q q

−q∗ q0

)

and E−(q)�=

(

q0I3 − q q

−q∗ q0

)

(B.19)

Exercise B.8 Properties of E− and E+ matrices.Let q = (q,q0) denote a unit quaternion:

1. Show that E+(q) and E−(q) are orthogonal matrices, i.e.,

E∗−(q)E−(q) = E−(q)E∗−(q) = I

E∗+(q)E+(q) = E+(q)E∗+(q) = I(B.20)

2. Show that

E∗−(q)E+(q) = E+(q)E∗−(q) =

(

R(q) 0

0 1

)

�= T(q)

E∗+(q)E−(q) = E−(q)E∗+(q) =

(

R∗(q) 0

0 1

)

= T(q−1) (B.21)

The T(.) homogeneous transforms defined above have zero translation vectorsand only rotational components.

3. Verify that

E+(q−1) = E∗+(q) and E−(q−1

) = E∗−(q) (B.22)

4. With p denoting another unit quaternion, show that

E+(p)q = E−(q)p (B.23)

5. With p denoting another unit quaternion, show that each of the E−(q) andE∗−(q) pair of matrices commutes with each of the E+(p) and E∗+(p) matrixpair, i.e.,

E−(q)E+(p) = E+(p)E−(q) (B.24a)

E−(q)E∗+(p) = E∗+(p)E−(q) (B.24b)

E∗−(q)E+(p) = E+(p)E∗−(q) (B.24c)

E∗−(q)E∗+(p) = E∗+(p)E∗−(q) (B.24d)


6. Show that q is an eigen-vector of T(q) with eigenvalue 1, i.e.,

T(q)q = q (B.25)

B.3.2 Quaternion Transformations

We denote the composition operation between two quaternions by the symbol “⊗”.The composition of two quaternions p and q is defined as

p⊗q�= E+(p)q

B.23= E−(q)p (B.26)

Exercise B.9 Composition of unit quaternions.Let q = (q,q0) and p = (p,p0) denote unit quaternions:

1. Verify that,

p⊗q =

[

p0q+pq0 + pq

p0q0 −p∗q

]

(B.27)

Verify that p⊗q is of unit norm and is hence also a unit quaternion. Thus, thecomposition of two quaternions yields another quaternion.

2. For quaternion composition to be consistent with the composition of rotationmatrices, we must have

R(p⊗q) = R(p) R(q) (B.28)

Show that this identity holds.3. Show that the composition of quaternions is an associative operation. That is,

with r denoting another quaternion, we have,

p⊗ (q⊗r) = (p⊗q)⊗r (B.29)

4. Show thatp−1 ⊗q = E∗+(p)q and p⊗q−1

= E∗−(q)p (B.30)

Exercise B.10 The quaternion identity element.Show that the eq = (0,0,0,1)∗ unit quaternion is the identity element for unit

quaternions by establishing the following identities for an arbitrary quaternion q:

q⊗eq = q and q⊗q−1= eq (B.31)


Exercise B.11 Unit quaternion products and inverses.For a pair of quaternions p and q, show that:

1. (p⊗q)−1 = q−1 ⊗p−1

2. With r�= p⊗q, show that,

q = p−1 ⊗r and p = r⊗q−1 (B.32)

Exercise B.12 Transforming vectors with unit quaternions.Given a vector x ∈R3, and a quaternion q = Iq

Bdefining the attitude of frame B

with respect to frame I, show that[

Ix

0

]

= q⊗[

Bx

0

]

⊗q−1 (B.33)

B.3.3 Quaternion Differential Kinematics

Exercise B.13 Quaternion rates from the angular velocity.With Iq

B= q = (q,q0), Show that the time derivative of the quaternion is related

to the Bω angular velocity as follows:

IqB

˙=

12

[

(q0I3 + q)Bω

−q∗Bω

]

=12

(

−B ω Bω

−Bω∗ 0

)

q

=12E+(q)

[

Bω

0

]

=12q⊗

[

Bω

0

]

(B.34)

Exercise B.14 Constant unit quaternion norms.

1. Show that the solution to the (B.34) ordinary differential equation has unit normfor all time t.

2. Show that when q0 �= 0,q0 = −q∗ q/q0 (B.35)


Exercise B.15 Unit quaternion rate to angular velocity.Let Iq

B= q = (q,q0) denote a unit quaternion:

1. Show that the mapping from quaternion rates to the Bω angular velocity repre-sentation is given by the following expression:

[

Bω

0

]

= 2E∗+(q) q = 2q−1 ⊗ q (B.36)

Alternatively,Bω= 2 [q0I3 − q] q−qq0 (B.37)

When q0 �= 0,Bω= 2

[

q0I3 − q+qq∗/q0]

q (B.38)

2. Show that the mapping from quaternion rates to the Iω angular velocity repre-sentation is as follows:

[

Iω

0

]

= 2E∗−(q) q = 2q⊗q−1 (B.39)

Alternatively,Iω = 2

[

q0I3 + q]

q−qq0 (B.40)

When q0 �= 0,Iω = 2

[

q0I3 + q+qq∗/q0]

q (B.41)

Exercise B.16 Quaternion double time derivatives.Let Iq

B= q = (q,q0) denote a unit quaternion. Show that

q =12q⊗

[

Bω

0

]

+14‖Bω‖2q (B.42)

Establish the converse relationship[

Bω

0

]

= 2q−1 ⊗ q−12‖Bω‖2eq (B.43)


B.4 Gibbs Vector Attitude Representations

Now we use the connection between rotation matrices and skew-symmetric matricesto define the Gibbs vector attitude representation [172].

Exercise B.17 Gibbs vector attitude representation.Let s denote a 3-vector with magnitude σ:

1. Use the characteristic polynomial for skew-symmetric matrices in Exercise B.2to show that

[I − s]−1= I +( s2

+ s)/(1 +σ2)

= I + s(I + s)/(1 +σ2) (B.44)

2. Use this identity to show that

[I − s]−1[I + s] = I + 2( s2

+ s)/(1 +σ2) (B.45)

Verify that R(s)�= [I − s]−1[I + s] is a rotation matrix. Such a representation

of attitude matrices using a 3-vector, s, is also known as the Rodrigues or Gibbsvector representation for attitude matrices.

3. Establish the converse relationship

s = −[

I −R(s)] [

I +R(s)]−1

= −[

γI −(1 +γ)R(s)+R2(s)

]

/(1 +γ) (B.46)

where γ�= Trace {R(s)}.

4. Show that the Gibb’s vector, s, is the axis of rotation for the R(s) rotation matrix.5. Show that the Gibb’s vector, s, for a quaternion q = (q,q0), with q0 �= 0, is given

bys= q/q0 = tan(θ/2)n (B.47)

so that R(s) = R(q) = R(n,θ).

We now look at the problem of finding the family of rotation matrices that willtransform one vector into another [15]. Let a and b denote two vectors of identicalnorm, i.e., ‖a‖ = ‖b‖. The goal is to find a parametrization of all rotation matricesR such that Ra= b.

Exercise B.18 Rotation of vectors.For a pair of 3-vectors a and b of the same norm, let s denote a Gibb’s vector

attitude representation such that R(s)a= b:

1. Show that s must satisfy the following relationship:

˜(a+b)s = (a−b) (B.48)

B.4 Gibbs Vector Attitude Representations 413

2. Now show that the general solution for s satisfying R(s)a= b is given by

s= λ ˜(a−b)(a+b)+α(a+b) (B.49)

where λ = 1/‖(a+b)‖2, and α is an arbitrary constant.

This exercise shows how to compute a rotation matrix that transforms a givenvector,a, into another given vector, b. This rotation matrix is not unique, as reflectedby the arbitrary parameter α in (B.49). After being rotated to coincide with b, anyadditional rotation of a about the axis b leaves the vector unchanged, leading to thenon-uniqueness of the rotation. A similar circumstance holds if vector a is rotatedabout itself prior to rotating it to coincide with b.

Appendix CSolutions

Solutions for Chapter 1

Solution 1.1 (pp. 8): Position and velocity vector derivatives

1.dBω(B)

dt1.12= α(B)+ ω(B)ω(B) = α(B)

This proves the first part of (1.17). The second expression follows since the an-gular velocity of I with respect to B is −ω(B) in (1.12).

2. Equation (1.18) follows by differentiating (1.15) with respect to the inertial frameand using (1.12) for the inertial derivative of l(x,y).

3. Equation (1.19a) follows from noting that δv =dBl(y,P)

dt and thus,

v(P) = v(y)+dIl(y,P)

dt1.12= v(y)+δv+ ω(B)l(y,P) (C.1)

The last term in (1.19a) vanishes because l(y,P) is instantaneously zero.Equation (1.19b) follows by differentiating (C.1) with respect to the inertialframe.

Solution 1.2 (pp. 9): Spatial vector cross-product identitiesThe identities can be established by direct verification using the 3-vector compo-nents of the A and B spatial vectors.

Solution 1.3 (pp. 10): Relationship between the ˜( ), ( ) and ( ) op-erators

These (1.27) equations can be established by direct verification for arbitrary spatialvectorsA, B and C.

415

416 C Solutions

Solution 1.4 (pp. 10): Time derivative relationship for spatial vectors

With X=

[

x

y

]

, we have

dFX

dt=

[

dFxdt

dFydt

]

1.12=

[

dGxdt + ωx

dGydt + ωy

]

1.21,1.23=

dGX

dt+ ˜Vω(F,G) X

Solution 1.5 (pp. 12): Identities involving ˜(·), (·) and φ(·, ·)The first two identities can be established by direct verification using arbitrary3-vectors l(x,y) andX. The last one is a simple rearrangement of the second identity.

Solution 1.6 (pp. 12): Rigid body transformation of ˜V(x)

From (1.33) on page 12 we know that V(y) = φ∗(x,y)V(x). Thus,

˜V(y)1.34= φ∗

(x,y)˜V(x)φ−∗(x,y)

and the first equation follows. The latter equation is merely a transposed version ofthe first equation, followed by the use of (1.26).

Solution 1.7 (pp. 13): Relationships involving Xω and Xv

The first pair of identities can be verified directly by expanding out X and Y intotheir component 3-vectors and evaluating them.Equation (1.36b) follows directly from (1.36a).Equation (1.36c) follows by direct verification.Equation (1.36d) follows from (1.36c).Equation (1.36e) can be verified directly by additionally using φ(x,y) for an arbi-trary l(x,y) 3-vector.

Solution 1.8 (pp. 13): The inertial frame derivative of φ(x,y)For the inertial frame derivative, we use the inertial frame representation of φ(x,y)from (1.30). Now,

dIl(x,y)dt

= v(y)−v(x)

Hence,dIφ(x,y)

dt=

[

0 v(y)− v(x)

0 0

]

1.25,1.21= V

v(y)− V

v(x)

C Solutions 417

Solution 1.9 (pp. 14): Local time derivative of φ∗(x,y)

1. We havevF(F,G)

1.11= vG(F,G)+ ω(F,G)l(F,G) (C.2)

Equation (1.39) follows from using the above in (1.40).2. The latter equality in (1.42) is a direct application of the last identity in (1.34) to

(1.39).For the first equality, differentiate (1.41) and use the chain rule to get

dφ∗(F,G)

dt1.41,1.12

=

(

ω(G,F)GRF 03

03 ω(G,F)GRF

)(

I3 03

−F˜l (F,G) I3

)

+

(

GRF 03

03GRF

)(

03 03

− vF(F,G) 03

)

1.41=

(

ω(G,F) 03

03 ω(G,F)

)

φ∗(F,G)

+

(

GRF 03

03GRF

)(

03 03

− vF(F,G) 03

)(

I3 03

−F˜l (F,G) I3

)

=

(

ω(G,F) 03

03 ω(G,F)

)

φ∗(F,G)

+

(

03 03

− vF(F,G) 03

)(

GRF 03

03GRF

)(

I3 03

−F˜l (F,G) I3

)

1.41=

(

ω(G,F) 03

03 ω(G,F)

)

φ∗(F,G)+

(

03 03

vF(G,F) 03

)

φ∗(F,G)

=

(

ω(G,F) 03

vF(G,F) ω(G,F)

)

φ∗(F,G) = ˜VF(G,F)φ∗

(F,G)

3. We have

φ∗(G,F)

dφ∗(F,G)

dtφ∗

(G,F)1.42= φ∗

(G,F)˜VF(G,F)φ∗(F,G)φ∗

(G,F)

= φ∗(G,F)˜VF(G,F) = −φ∗

(G,F)˜VF(F,G)

This expression agrees with the one in (1.43) and establishes the result.

418 C Solutions

4. We have that

dφ∗(G,H)

dtφ∗

(F,G)+φ∗(G,H)

dφ∗(F,G)

dt1.42= ˜VG(H,G)φ∗

(G,H)φ∗(F,G)+φ∗

(G,H)φ∗(F,G)˜VG(G,F)

= ˜VG(H,G)φ∗(F,H)+φ∗

(F,H)˜VG(G,F)

=

(

ω(H,F) 03

vG(H,G)− ω(H,G)˜l(F,H)+ vG(G,F)−˜l(F,H)ω(G,F) ω(H,F)

)

=

(

ω(H,F) 03

vG(H,F)− ω(H,G)˜l(F,H)− vG(G,F)−˜l(F,H)ω(G,F) ω(H,F)

)

Using (C.2) in the lower left expression, for vG(H,F) and simplifying we obtain

dφ∗(G,H)

dtφ∗

(F,G)+φ∗(G,H)

dφ∗(F,G)

dt

=

(

ω(H,F) 03

vF(H,F)− ω(H,F)˜l(F,H) ω(H,F)

)

= ˜VF(H,F)φ∗(F,H)

1.42=

dφ∗(F,H)

dt

establishing the result.


Solution 2.1 (pp. 19): Rigid body center of massWe have l(x,a) = l(x,y)+ l(y,a). Therefore,

p(x)2.8=

1m

∫Ω

l(x,a)ρ(a)dϑ(a)

=1m

∫Ω

l(x,y)ρ(a)dϑ(a)+1m

∫Ω

l(y,a)ρ(a)dϑ(a)2.8= l(x,y)+p(y)

C Solutions 419

Solution 2.2 (pp. 19): Parallel-axis theorem for rotational inertias

1. We have

J (x)2.8= −

∫Ω

˜l(x,a)˜l(x,a)ρ(a)dϑ(a)

= −

∫Ω

˜l(x,C)˜l(x,C)ρ(a)dϑ(a)−

∫Ω

˜l(C,a)˜l(C,a)ρ(a)dϑ(a)

−

∫Ω

˜l(x,C)˜l(C,a)ρ(a)dϑ(a)−

∫Ω

˜l(C,a)˜l(x,C)ρ(a)dϑ(a)

2.8= J (C)−m p(x) p(x)−m˜l(x,C) p(C)−m p(C)˜l(x,C)

= J (C)−m p(x) p(x)

The last step used the fact that p(C) = 0.2. The symmetry of J (x) is easy to verify from its definition. Its positive semi-

definiteness follows from the positive semi-definiteness of the integrand−˜l(x,k)˜l(x,k)ρ(k). Since −mp(x) p(x) = m[ p(x)]∗ p(x) is always positivesemi-definite implies that J (x) � J (C) for all points x.

3. For J (x) to fail to be positive definite, there must exist a non-zero vector y suchthat y∗J (x)y = 0. That is,

0 = y∗J (x)y2.8= −

∫Ω

y∗˜l(x,a)˜l(x,a)yρ(a)dϑ(a)

=

∫Ω

z∗(a)z(a)ρ(a)dϑ(a) where z(a)�= ˜l(x,a)y

The above integral can vanish in the following cases:

Point mass: Here ρ(a) = 0 for all l(x,a) �= 0 and J (x) ≡ 0.Infinitely thin rod along y: Here ρ(a) = 0 for all l(x,a) �= ky for some scalark �= 0.

Solution 2.3 (pp. 20): Positive semi-definiteness of spatial inertiasJ (C) being positive definite or positive semi-definite implies the same property forM(C). The same property also applies toM(x) = φ(x,C)M(C)φ∗(x,C) in (2.12)since φ(x,y) is always non-singular.

420 C Solutions

Solution 2.4 (pp. 21): Invariance of the kinetic energyFrom (2.12), we have

Ke =12V∗

(x)M(x)V(x)2.12=

12V∗

(x)φ(x,y)M(y)φ∗(x,y)V(x)

1.33=

12V∗

(y)M(y)V(y)

Since the points x and y are arbitrary, this establishes the invariance of the kineticenergy.

Solution 2.5 (pp. 22): Relationship of spatial momenta about points xand y

h(x)2.16= M(x)V(x)

2.12= φ(x,y)M(y)φ∗

(x,y)V(x)

1.33= φ(x,y)M(y)V(y)

2.16= φ(x,y)h(y)

Solution 2.6 (pp. 25): Time derivative of rigid body spatial inertiaWe have

IM(z) =

(

IRB 0

0 IRB

)

BM(z)

(

BRI 0

0 BRI

)

Differentiating this equation while noting that BM is constant leads to:

M(z) =

(

ω 0

0 ω

)

BM(z)−M(z)

(

ω 0

0 ω

)

= ˜Vω(z)M(z)−M(z)˜Vω(z)

Solution 2.7 (pp. 26): bI(C) gyroscopic spatial force does no workWe can verify that bI(z) does no work by taking its dot product with the spatialvelocity vector to obtain:

V∗(C)bI(C)

2.23=

[

ω∗(ωJ (C)ω)

0

]

= 0

C Solutions 421

Solution 2.8 (pp. 27): Inertial generalized accelerations at two points

βI(y)�=

dIV(y)

dt=

dI[φ∗(x,y)V(x)]

dt

= φ∗(x,y)βI(x)+

dI[φ∗(x,y)]dt

βI(x)

1.37= φ∗

(x,y)βI(x)+

[

− ˜V(y)+ ˜V(x

]

V(x)

= φ∗(x,y)βI(x)− ˜V(y)V(x) = φ∗

(x,y)βI(x)+

[

0ω [v(y)−v(x)]

]

1.15= φ∗

(x,y)βI(x)+

[

0

ωω l(x,y)

]

Solution 2.9 (pp. 28): bI(z) gyroscopic spatial force does workWe can verify that bI(z) does work by taking its dot product with the spatial velocityvector as follows:

V∗(z)bI(z)

2.26= V∗

(z)˜Vω(z)M(z)Vω(z)

This expression is non-zero in general and hence, the gyroscopic force does work.Assuming that the spatial force is zero, i.e., f(z) ≡ 0, the time derivative of thekinetic energy of the body is:

dKe

dt=

12

dβ∗I(z)M(z)βI(z)

dt= β∗

I(z)M(z)βI(z)+12β∗

I(z)M(z)βI(z)

2.26,2.22= β∗

I(z) [f(z)−bI(z)]−β∗I(z)M(z)˜Vω(z)βI(z)

2.26= −mv∗(z)ωωp(z)−mω∗ p(z)ωv(z) = 0

This proves that the kinetic energy is conserved in the absence of external forces.

Solution 2.10 (pp. 28): Non-conservation of spatial momentumFor any point x on the rigid body, from (2.17), h(x) = φ(x,C)h(C). Differentiatingthis equation with respect to time in the inertial frame leads to

dIh(x)

dt=

dIφ(x,C)

dth(C)+φ(x,C)

dIh(C)

dt2.21=

dIφ(x,C)

dth(C)+φ(x,C)f(C)

1.15,1.37=

[

m[ v(C)− v(x)]v(C)

0

]

+f(x) =

[

m v(C)v(x)

0

]

+f(x)

= −Vv(x)h(C)+ f(x) (C.3)

422 C Solutions

The Vv(x)h(C) quantity vanishes if and only if either: (a) the point x coincides with

C; (b) v(C) or v(x) is zero, i.e., the body is spinning about the l(x,C) vector. Thisestablishes (2.27).

Solution 2.11 (pp. 30): Equations of motion using spatial momentumWe have

dIh(x)

dt1.28= V

ω(x)h(x)+

dBh(x)

dt= V

ω(x)h(x)+

dBM(x)βB(x)

dt= V

ω(x)h(x)+M(x)βB(x)

Combining this with (C.3) leads to

f(x) =M(x)βB(x)++

[

Vω

(x)+ Vv(x)

]

h(x)1.22= M(x)βB(x)+ V(x)h(x)

This agrees with (2.28) and establishes the result.

Solution 2.12 (pp. 32): Invariance of VI to velocity reference pointNow

φ∗(x,I)V(x)

1.33= φ∗

(x,I)φ∗(C,x)V(C)

1.32= φ∗

(C,I)V(C)2.30= VI


Solution 2.13 (pp. 32): Time derivative ofMI

Differentiating (2.32) we have

dIMI

dt2.32= φ(I,C)

dIM(C)

dtφ∗

(I,C)+dIφ(I,C)

dtM(C)φ∗

(I,C)

+φ(I,C)MI

dIφ∗(I,C)

dt2.22,1.37

= φ(I,C)

[

VωM(C)−M(C)˜Vω

]

φ∗(I,C)

+ Vv(C)M(C)φ∗

(I,C)−φ(I,C)M(C)˜Vv(C)

(C.4)

However, from (1.36) we have

Vv(C) = φ(I,C)V

v(C)

C Solutions 423

and obtain

dIMI

dtC.4= φ(I,C)

[

Vv(C)+ V

ω(C)

]

M(C)φ∗(I,C)

−φ(I,C)M(C)

[

˜Vv(C)+ ˜Vω(C)

]

φ∗(I,C)

1.25= φ(I,C)V(C)M(C)φ∗

(I,C)−φ(I,C)M(C)˜V(C)φ∗(I,C)

1.35,2.30= VIφ(I,C)M(C)φ∗

(I,C)−φ(I,C)M(C)φ∗(I,C)˜VI

2.32= VIMI −MI

˜VI

Solution 2.14 (pp. 33): Equations of motion about a fixed velocityreference point

1. We have

dIhI

dt2.34=

dIφ(I,C)

dth(C)+φ(I,C)

dIh(C)

dt2.21,1.37

= Vv(C)h(C)+φ(I,C)f(C)

2.35= 0+fI = fI

The last equality above used the Vv(C)h(C) = 0 identity.

2. From (2.36) we have

fI2.36=

dIhI

dt2.34= MIβI +

dIMI

dtVI

2.33= MIβI − VIMIVI

3. We can verify that bI is non-working by taking its dot product with VI and not-ing that V∗

IVI = 0. Recognizing that the kinetic energy can also be written as

12β

∗IMIβI and differentiating it with respect to time leads to the conclusion that

it is conserved in the absence of external forces.


Solution 3.1 (pp. 41): Hinge map matrix for a universal jointThe orientation of the second axis in a universal joint depends on the general coor-dinate of the first axis. The hinge map matrix for the universal joint is

H∗=

⎛

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎜

⎝

1 0

0 cosθ1

0 sinθ1

0 0

0 0

0 0

⎞

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎟

⎠

424 C Solutions

Solution 3.2 (pp. 41): Hinge map matrix for a sphere rolling on asurface

Let ΔV denote the spatial velocity of the frame fixed to the center of the sphere.Also, let l denote the vector from the center of the sphere to the instantaneous pointof contact. The orientation of this vector is constant in the inertial frame but ischanging in the body frame. The zero linear velocity of the contact point translatesto the following constraint expression:

Δv+ ˜Δωl = 0 ⇒[

−˜l, I3

]

ΔV = 03 where ΔV�=

[

Δω

Δv

]

where Δω and Δv are the angular and linear velocity components of ΔV at thecenter of the sphere. This implicit 3-dimensional constraint implies thatΔV must beof the following form:

ΔV =

[

I3˜l

]

β

where β ∈ R3 is the vector of generalized velocities for this contact hinge. Thus,with β = Δω as the generalized velocity coordinates, the hinge map matrix for therolling contact is given by

H∗=

[

I3˜l

]

∈R6×3

Solution 3.3 (pp. 46): Velocity recursion with Bk �= Ok

In this context, the recursion in (3.19b) can be more precisely expressed as:

V(Ok) = φ∗(Ok+1,Ok)V(Ok+1)+H∗

(k)θ(k)

Now use the following versions of (3.20) and (3.23) in the equation above to estab-lish the result:

V(k)�= V(Bk) = φ∗

(Ok,Bk)V(Ok)

and V(k+ 1)�= V(Bk+1) = φ∗

(Ok+1,Bk+1)V(Ok+1)

Solution 3.4 (pp. 47): Velocity recursion with inertially fixedreference point

We have

VI(k)3.24= φ∗

(Ok,I)V(Ok)

3.19b= φ∗

(Ok,I) {φ∗(Ok+1,Ok)V(Ok+1)+H∗

(k)β(k)}

C Solutions 425

3.24= φ∗

(Ok,I) {φ∗(Ok+1,Ok)φ

∗(Ok+1,I)VI((k+ 1))+H∗

(k)β(k)}

3.25= VI(k+ 1)+H∗

I (k)β(k)3.25= VI(k+ 1)+ΔI

V(k)β(k)


Solution 3.5 (pp. 50): Internal structure of the φ operatorSince (I−Eφ) is lower-triangular, and has identity matrices along the diagonal, thesame holds true for its inverse, φ, as well establishing (3.37). Now, let us exam-ine the lower-triangular component elements of the matrices on both sides of theoperator identity:

φ−Eφφ= I

This identity implies that for i > j,

φ(i, j)−φ(i, i− 1)φ(i− 1, j) = 0 ⇒ φ(i, j) = φ(i, i− 1)φ(i− 1, j)

Applying this next toφ(i−1, j), and continuing on in this vein, leads to the first halfof (3.38). Since the product of a sequence of rigid body transformation matrices isalso a rigid body transformation matrix, the latter equality in (3.38) follows as well.

Solution 3.6 (pp. 51): The φ spatial operator

1. For a matrix A such that (I−A) is invertible, (A.17) on page 400 states that:

A(I−A)−1

= (I−A)−1A = (I−A)

−1− I

(3.41) is a consequence of this identity with A= Eφ and φ= (I−Eφ)−1.2. From (3.15) it follows that

V+ 3.15= E∗

φV3.39= E∗

φφ∗H∗ θ 3.41

= φ∗H∗ θ

Solution 3.7 (pp. 53): Recursive evaluation of φx and φ∗xWith y= φx, we have y

�= φx= Eφy. This implies that y(k) = φ(k,k− 1)y(k).

Thus, the computational algorithm for y is the one in (3.43) except that the compu-tational step in the loop is now given by

y(k) = φ(k,k− 1)[y(k− 1)+x(k− 1)]

with initial condition y(0) = 0.

Similarly, with y= φ∗x, we have y�= φ∗x= E∗

φy. Thus,

y(k) = φ∗(k+ 1,k)y(k+ 1)

426 C Solutions

Thus, the computational algorithm for y is the same as in (3.45) except that thecomputational step within the loop has the form:

y(k) = φ∗(k+ 1,k)[y(k+ 1)+x(k+ 1)]

with initial condition y(n+ 1) = 0.


Solution 4.1 (pp. 63): System center of mass

1. The overall spatial inertia of the system referenced to the base-body frame isobtained by using the parallel axis theorem for spatial inertias to reference all thelink spatial inertias to Bn and summing them up. That is,

MS =

n∑k=1

φ(n,k)M(k)φ∗(n,k)

4.4= [φ(n,1), · · · φ(n,n)]M

⎡

⎢

⎢

⎣

φ∗(n,1)

...

φ∗(n,n)

⎤

⎥

⎥

⎦

3.37= EφMφ∗E∗ 4.10

= E[

R+ φR+Rφ∗]E∗ = ERE∗ = R(n)

establishing the result. We have used the following facts in the above derivation:

Eφ3.37= [φ(n,1), · · · φ(n,n)] and φRE∗ = 0 (C.5)

2. The base-body frame referenced spatial momentum of the system is obtained byreferencing the spatial momentum for each of the bodies to the Bn frame andsumming them up. This leads to:

hS2.17=

n∑k=1

φ(n,k)h(k)2.16=

n∑k=1

φ(n,k)M(k)V(k)C.5= EφMV

3.39= EφMφ∗H∗ θ 4.10

= E[

R+ φR+Rφ∗]H∗ θ C.5= EφRH∗ θ

For a free flying system, H∗(n) = I and θ(n) = V(n), and hence hS can bere-expressed as

hS = R(n)V(n)+

n−1∑k=1

φ(n,k)R(k)H∗(k)θ(k)

3. The system level spatial momentum hS, the system level spatial inertia MS andthe center of mass velocity VC (referenced about Bn) are related together by

C Solutions 427

hS =MSVC

4.13= R(n)VC (C.6)

4. For a free-flying system, we have,

VC

4.16= R−1

(n)

[

R(n)V(n)+

n−1∑k=1


]

= V(n)+R−1(n)

n−1∑k=1


establishing (4.18).From (4.16) it follows directly that adding δV to the V(n) base-body spatialvelocity will result in an additional R(n)δV of spatial momentum system to thesystem. Now for the spatial momentum to be zero, we must have

0 = hS+R(n)δV =⇒ δV = −R−1(n)hS

C.6= −VC

Solution 4.2 (pp. 65): Trace of the mass matrixFrom the decomposition of the mass matrix in (4.19), and noting that the trace ofstrictly lower- and upper-triangular matrices is zero, we have

Trace {M(θ)}4.19= Trace

{HRH∗

+HφRH∗+HRφ∗H∗}

= Trace {HRH∗} =

n∑i=1

Trace {H(k)R(k)H∗(k)}

In the above we have used the zero trace property of HφRH∗ which is a conse-quence of its strictly lower-triangular structure.

Solution 4.3 (pp. 68): The MD(θ, θ) matrix

1. We have

M∗D θ

4.29= [∇θ(Mθ)]∗ θ A.24

=

[

∂Mθ

∂θ(1), · · · ∂Mθ

∂θ(n)

]∗θ

=

⎡

⎢

⎢

⎢

⎣

∂θ∗M

∂θ(1)...

∂θ∗M

∂θ(n)

⎤

⎥

⎥

⎥

⎦

θ =

⎡

⎢

⎢

⎢

⎣

∂θ∗Mθ

∂θ(1)...

∂θ∗Mθ

∂θ(n)

⎤

⎥

⎥

⎥

⎦

A.27=

∂

∂θ(θ

∗Mθ)

428 C Solutions

2.

MD(θ, θ)θ4.29=

[

∂Mθ

∂θ(1), · · · ∂Mθ

∂θ(n)

]∗θ =

N∑k=1

∂Mθ

∂θ(k)θ(k)

=

[

N∑k=1

∂M

∂θ(k)θ(k)

]

θ = M(θ)θ

Solution 4.4 (pp. 68): Mθ− 2C is a non-working force

1. Equation (4.33) follows from combining (4.31) and (4.32).2. The work done by a generalized force is given by its dot product with the θ

generalized velocities vector. Thus, the work done by Mθ− 2C is given by

θ∗ [

Mθ− 2C] 4.33

= θ∗(M∗

D−MD)θ = 0

The last equality follows since (M∗D−MD) is skew-symmetric.

3. The work done by the C Coriolis generalized forces vector is given by

θ∗C(θ, θ)

4.32= θ

∗[

MD−12M∗D

]

θ=12θ∗MD θ

SinceMD is not skew-symmetric, the work done by C is non-zero in general.

Solution 4.5 (pp. 68): Rate of change of the kinetic energy

dKe

dt4.25= θ

∗Mθ+

12θ∗Mθ

4.26,4.31= θ

∗[

(T −C)+12MD θ

]

4.32= θ

∗[

T −(MD−12M∗D)θ+

12MD θ

]

4.32= θ

∗[

T −12(MD−M∗

D)θ

]

= θ∗T

The last step follows since (MD−M∗D) is skew-symmetric.

Solution 4.6 (pp. 69): Christoffel symbols of the first kind

1. The identities in (4.36) follow from simply comparing the definitions of Ci(j,k),Ci(k, j) and Ci(i,k).For (4.37),

C Solutions 429

Cj(i,k)+Ck(j, i)4.35=

12

[

∂M(i, j)∂θ(k)

+∂M(j,k)∂θ(i)

−∂M(i,k)∂θ(j)

]

+12

[

∂M(k, j)∂θ(i)

+∂M(k, i)∂θ(j)

−∂M(j, i)∂θ(k)

]

When the mass matrix is symmetric, the above expression simplifies to ∂M(k,j)∂θ(i) .

2. For multibody systems, the mass matrix is symmetric, and also ∂M(i,k)∂θ(j) = 0 for

j � i,k as discussed in Remark 4.1. Using these properties in the definition ofthe left- and right-hand sides of the identities in (4.38) establishes the identities.Moreover, since Ci(j,k) depends only upon M(i, j), M(i,k) and M(j,k), andthese terms depend only upon θ(l) for l < max(i, j,k), this implies that Ci(j,k)depends only upon θ(l) for l < max(i, j,k).

3. We have

C(i)4.27=

{M(θ)θ−

12∂

∂θ[θ

∗M(θ)θ]

}(i) =

N∑k=1

M(i,k)θ(k)−12θ∂M

∂θ(i)θ

=

N∑j,k=1

∂M(i,k)∂θ(j)

θ(j)θ(k)−12

N∑j,k=1

∂M(j,k)∂θ(i)

θ(j)θ(k)

=

N∑j,k=1

[

∂M(i,k)∂θ(j)

−12∂M(j,k)∂θ(i)

]

θ(j)θ(k)

=

N∑j,k=1

12

[

∂M(i,k)∂θ(j)

+∂M(i, j)∂θ(k)

−12∂M(j,k)∂θ(i)

]

θ(j)θ(k)

4.35=

N∑j,k=1

Ci(j,k)θ(j)θ(k) = θ∗Ciθ

4. We have

MD(i, j)4.29=

∂[Mθ](i)

∂θ(j)=

N∑k=1

∂M(i,k)∂θ(j)

θ(k)4.37=

N∑k=1

[Ci(j,k)+Ck(j, i)]θ(k)

Solution 4.7 (pp. 70): Hamiltonian form of the equations of motion

We have∂H

∂p

4.42= M−1

p4.28= θ

430 C Solutions

This establishes the first expression in (4.43). Also,

∂H

∂θ

4.42=

12

col{p∗ ∂M−1

∂θ(k)p

}N

k=1

4.42,A.28= −

12

col{

p∗M−1 ∂M

∂θ(k)M−1

p

}N

k=1

4.28= −

12

col{∂θ

∗Mθ

∂θ(k)

}N

k=1

4.28= −

12∂θ

∗Mθ

∂θ

4.27= −Mθ+C

4.28= −p+Mθ+C

4.24= −p+T

This establishes the latter expression in (4.43).

Solution 4.8 (pp. 71): Lagrangian equations of motion using quasi-velocities

We have

∂L

∂θ

A.27= [∇

θL]

∗ A.26= [∇βL ·∇

θβ]

∗= [∇βL ·A]

∗ A.27= A∗

(θ)∂L

∂β(C.7)

Therefore, differentiating this with respect to t we have

ddt∂L

∂θ

C.7= A∗

[

ddt∂L

∂β

]

+ A∗ ∂L∂β

(C.8)

Similarly,

∂L

∂θ

A.27= [∇θL]

∗ A.26= [∇βL ·∇θβ+∇θL]

∗ A.27=

∂β

∂θ

∂L

∂β+∂L

∂θ(C.9)

Combining (C.8) and (C.9) it follows that

T4.24=

ddt∂L

∂θ−∂L

∂θ=A∗

[

ddt∂L

∂β

]

−∂L

∂θ+

[

A∗−∂β

∂θ

]

∂L

∂β

=A∗[

ddt∂L

∂β

]

−∂L

∂θ+γ∗

∂L

∂β

and therefore

A−∗T =

[

ddt∂L

∂β

]

−A−∗∂L∂θ

+A−∗γ∗∂L

∂β

establishing (4.44). Now observe that

A(θ) =

n∑i=1

∂A

∂θ(k)θ(k) and

∂β

∂θ=∂Aθ

∂θ(C.10)

C Solutions 431

Thus,

γ(i, j) = A(i, j)−∂β(i)

∂θ(j)=

n∑k=1

∂A(i, j)∂θ(k)

θ(k)−∂

∑nk=1A(i,k)θ(k)∂θ(j)

=

n∑k=1

(

∂A(i, j)∂θ(k)

−∂A(i,k)∂θ(j)

)

θ(k)

This establishes (4.46).

Solution 4.9 (pp. 72): Lagrangian equations of motion under coordi-nate transformations

Since η= h(θ), thereforeη= A(θ)θ

Thus,A(i, j) =∂h(i)∂θ(j) and

∂A(i, j)∂θ(k)

=∂A(i,k)∂θ(j)

=∂2h(i)

∂θ(j)∂θ(k)

Therefore, the (4.46) expression for the γ(i, j) elements are zero. Hence, γ= 0.

Solution 4.10 (pp. 72): Non-working Mβ− 2C generalized forceWe have

β∗A−∗ ∂L∂θ

= θ∗ ∂L∂θ

=12θ∗[∇θβ∗M(θ)β]

∗=

12

[∇θβ∗M(θ)β] θ

=12β∗M(θ)β

(C.11)

Also,γA−1β = γθ

4.48= = 0 (C.12)

Therefore

β∗(Mβ− 2C)

4.50= β∗

(

−Mβ+ 2A−∗∂L∂θ

− 2A−∗γ∗∂L

∂β

)

C.11,C.12= −β∗Mβ+β∗Mβ− 0 = 0

432 C Solutions

Solution 4.11 (pp. 73): Nonlinear diagonalizing coordinate transfor-mations

1. (4.49) on page 72 describes the effect of a coordinate transformation upon theLagrangian equations of motion which in the (η, η) coordinates are given by

ddt∂L

∂η−∂L

∂η=A−∗T (C.13)

Since M(θ) = A∗(θ)A(θ), this means that

Ke = L(θ, θ) = L(η, η) =12η∗ η and

∂L

∂η= 0 (C.14)

Using these in (C.13) leads to

η= A−∗T = ζ

establishing (4.51).2. This time

Ke = L(θ,β) =12β∗β and

∂L

∂θ= 0 (C.15)

Sinceβ are now quasi-velocities, the appropriate Lagrangian equations of motionare those in (4.44) and using (C.15) take the form:

β−A−∗γ∗β= ζ

and the result follows.3. From Exercise 4.10 we have

0 = β∗(Mβ− 2C) = −2β∗C

The above uses the fact that M is the constant identity matrix. This implies thatthe C(θ,β) Coriolis forces vector do no work.


Solution 5.1 (pp. 79): a(k) for helical & cylindrical hingesThe hinge map matrix for a helical hinge with axis hω(k) and pitch p has the form

H∗(k) =

[

hω(k)

phω(k)

]

=⇒ ΔV(k) =

[

Δω(k)

pΔω(k)

]

C Solutions 433

Thus,

a(k)5.12= −

[

˜Δω(k)ω(k)

˜Δω(k){v(k)+pω(k)}

]

For a cylindrical hinge with axis hω(k), the axis of rotation and translation are thesame. For this case

H∗(k) =

(

hω(k) 00 hω(k)

)

=⇒ ΔV(k) =

[

Δω(k)

Δv(k)

]

=

[

hω(k)θ1(k)

hω(k)θ2(k)

]

Thus,

a(k)5.15= −

[

˜Δω(k)ω(k)

˜Δω(k)v(k)+ ˜Δv(k)ω(k)

]

Solution 5.2 (pp. 80): aB(k) with body frame derivatives but Bk �= Ok

Since the reference for the kth body does not coincide with Ok, we have from(3.20) that

V(k)�= V(Bk) = φ∗

(Ok,Bk)V(Ok) (C.16)

and the spatial acceleration αB(k) is defined as

αB(k)�=

dkV(k)

dt

Differentiating (C.16) with respect to the kth body frame and noting thatφ∗(Ok,Bk) is constant in the kth body frame, we have

αB(k)C.16= φ∗

(Ok,Bk)dkV(Ok)

dt= φ∗

(Ok,Bk)α(k)

5.8= φ∗

(Ok,Bk){φ∗

(Ok+1,Ok)φ∗(Bk+1,Ok+1)αB(k+ 1)

+H∗(k)θ(k)+a(k)

}= φ∗

(Bk+1,Bk)αB(k+ 1)+φ∗(Ok,Bk)H

∗(k)θ(k)+φ∗

(Ok,Bk)a(k)

3.22= φ∗

(Bk+1,Bk)αB(k+ 1)+H∗B(k)θ(k)+φ∗

(Ok,Bk)aB(k)

This establishes (5.18). Note further that

φ∗(Ok,Bk)˜V(Ok)ΔV(k)

1.35,C.16= ˜V(k)φ∗

(Ok,Bk)ΔV(k) (C.17)

and that

φ∗(Ok,k)ΔV(k)ΔV(k) = φ∗

(Ok,k)

[

0˜Δω(k)Δv(k)

]

=

[

0˜Δω(k)Δv(k)

]

= ΔV(k)ΔV(k)

(C.18)

434 C Solutions

Thus (5.18) can be re-expressed as:

aB(k)C.17,C.18,3.22

= ˜V(k)ΔB

V(k)− ΔV(k)ΔVk+φ∗(Ok,k)

dk+1H∗(k)

dtθ(k)


Solution 5.3 (pp. 81): aI(k) with inertial frame derivativesDifferentiating (5.6) with respect to the inertial frame I, we obtain

αI(k)�=

dIV(k)

dt1.28= α(k)+ ˜Vω(k)V(k)

1.36= α(k)+ V(k)V(k) (C.19)

Using (C.19) in (5.8) we have

αI(k) = φ∗(k+ 1,k)αI(k+ 1)+H∗

(k)θ(k)+aI(k) (C.20)

where the velocity dependent Coriolis acceleration term, aI(k), is given by

aI(k)�= a(k)+ V(k)V(k)− V(k+ 1)V(k+ 1)

This establishes the first equality in (5.19).For the latter equality in (5.19), we have

a(k)+ V(k)V(k)− V(k+ 1)V(k+ 1)

5.11= ˜V(k)ΔV(k)− ΔV(k)ΔV(k)+

dk+1H∗(k)

dtθ(k)

+ V(k)V(k)− V(k+ 1)V(k+ 1)

(C.21)

The individual terms in (C.21) can be re-expressed as follows:

˜V(k)ΔV(k) =

[

˜Vω(k)+ ˜Vv(k)]

ΔV(k)1.36= ˜Vω(k)ΔV(k)+ ˜Vv(k)ΔωV (k)

1.36=

[

˜Vω(k+ 1)+ ˜ΔωV (k)

]

ΔV(k)+ ˜Vv(k)ΔωV (k)

= ˜Vω(k+ 1)ΔV(k)+ ˜ΔωV (k)ΔvV(k)− ˜ΔωV (k)Vv(k)

ΔV(k)ΔV(k)1.36= ˜ΔωV (k)ΔvV(k)

V(k)V(k)1.36= ˜Vω(k)Vv(k) =

[

˜Vω(k+ 1)+ ˜ΔωV (k)

]

Vv(k)

V(k+ 1)V(k+ 1)1.36= ˜Vω(k+ 1)Vv(k+ 1)

Substituting the above expressions into (C.21) and simplifying we obtain the latterequality in (5.19).Equation (5.20a) is a component-level expansion of the first equality in (5.19).

C Solutions 435

The top angular half of (5.20b) is a simple carryover from a(k). For the lower linearhalf, observe from (5.12) and (5.20a) that

− ˜Δv(k)ω(k+ 1)− ˜Δω(k)v(k)+ ω(k)v(k)− ω(k+ 1)v(k+ 1)

= ω(k+ 1)[v(k)−v(k+ 1)+Δv(k)]

Solution 5.4 (pp. 88): Inclusion of gravitational forces

1. The application of a pseudo-acceleration g at the base-body results in the follow-ing altered version of the expression for α in (5.22):

α= E∗φα+H∗θ+a+E∗g

Using (3.36), we then obtain

α= φ∗[H∗ θ+a+E∗g]

Thus, the effect of the pseudo-acceleration can be handled by replacing a bya+E∗g. This leads to the Coriolis forces vector expression in (5.39).

2. Adding the additional gravitational force leads to the following body level equa-tions of motion:

f(k)5.1= φ(k,k− 1)f(k− 1)+M(k)α(k)+b(k)+M(k)g

= φ(k,k− 1)f(k− 1)+M(k)[α(k)+g]+b(k)

This in turn leads to the following altered expression for f in (5.22):

f = Eφf+M[α+E∗g]+b ⇒ f = φ[

M[α+E∗g]+b]

Using this expression for f with the rest of the expressions in (5.23) leads to(5.40).

3. We have thatφ∗E∗g = col

{φ∗

(n,k)g}nk=1

However, since g is a purely linear acceleration,φ∗(n,k)g = g. Hence, the aboveequation simplifies to E∗g and establishes (5.41).

436 C Solutions

Solution 5.5 (pp. 92): Inverse dynamics using composite bodyinertias

1.

f5.23= φ[Mφ∗

(H∗ θ+a)+b]4.10= (φR+Rφ)(H∗ θ+a)+φb

5.23= Rα+φ

[

b+EφR(Hθ+a)]

establishing (5.44).2. Define

y+ �= y+R(H∗ θ+a)

5.44= φ

[

b+EφR(H∗ θ+a)]

+R(H∗θ+a)

= φb+ φR(H∗ θ+a)+R(H∗θ+a) = φ[

b+R(H∗θ+a)]

(C.22)

Thus

Eφy+ C.22

= φ[

b+R(H∗θ+a)] 5.44

= y−b ⇒ y= Eφy+

+b

The first expression in the (5.45) recursion is a component level expression of theabove. The second expression follows from the y+ = y+R(H∗ θ+a) definitionof y+.

Solution 5.6 (pp. 94): Expression for H∗I(k)

dIH∗I(k)

dt3.25=

dIφ∗(Ok,I)dt

H∗(k)+φ∗

(Ok,I)dIH

∗(k)dt

1.28,1.37= ˜Vv(k)H∗

(k)+φ∗(Ok,I)

[

˜Vω(k)H∗(k)+

dk+1H∗(k)

dt

]

1.36= φ∗

(Ok,I)

[{˜Vv(k)+ ˜Vω(k)

}H∗

(k)+dk+1H

∗(k)dt

]

1.22= φ∗

(Ok,I)

[

˜V (k)H∗(k)+

dk+1H∗(k)

dt

]

5.48,1.35= ˜VI(k)φ

∗(Ok,I)H∗

(k)+φ∗(Ok,I)

dk+1H∗(k)

dt3.25= ˜VI(k)H

∗I(k)+φ∗

(Ok,I)dk+1H

∗(k)dt

C Solutions 437


Solution 6.1 (pp. 102): Properties of the τ(k) and τ(k) projectionmatrices

1. We have

τ(k) · τ(k) 6.16= G(k)H(k)G(k)H(k)

6.14= G(k)H(k) = τ(k)

This implies that τ(k) is a projection operator. Since τ(k) is a projection operator,it follows directly that so are τ(k) = I−τ(k) and their transposes.

2. These identities follow by using (6.14) and the definitions in (6.16).

Solution 6.2 (pp. 104): The G(k) Kalman gain and the link spatialacceleration

We have G∗(k)α(k)6.15= G∗τ∗(k)α+(k)

6.18= 0.

Solution 6.3 (pp. 105): Properties of P+(k)

1. We have

P+(k)

6.24= P(k)τ∗(k) 6.16

= P(k)[

I−H∗(k)G∗

(k)]

6.13= P(k)−P(k)H∗

(k)D−1(k)H(k)P(k)

=[

I−P(k)H∗(k)D−1

(k)H(k)]

P(k)

6.13=

[

I−G(k)H(k)]

P(k)6.16= τ(k)P(k)

That is,P+

(k) = P(k)τ∗(k) = τ(k)P(k) (C.23)

Thus

τ(k)P(k)τ∗(k) C.23= P(k)[τ∗(k)]2 = P(k)τ∗(k)

2. We have H(k)P+(k)6.25= H(k)τ(k)P(k)

6.18= 0. The latter equality is obtained

by transposing this equality and using the symmetry of P+(k).

438 C Solutions

Solution 6.4 (pp. 107): Ordering of R(k), P(k) andM(k)

We have

P+(k)

6.25= P(k)−τ(k)P(k)τ∗(k)

Since τ(k)P(k)τ∗(k) � 0 this implies that P(k) � P+(k).P(k) �M(k) follows from (6.28).For link 1, R(1) = P(1) =M(1), and hence R(1) � P(1). Now we prove that ifR(k) � P(k) then necessarily R(k+ 1) � P(k+ 1). For this, observe that R(k) �P(k) implies that R(k) � P+(k). Then comparing (4.8) on page 60 with (6.28)implies that R(k+ 1) � P(k+ 1).

Solution 6.5 (pp. 111): Relationship of ν(k) to α(k)

G∗(k)α(k)

6.43= G∗

(k)[

ψ∗(k+ 1,k)α(k+ 1)+H∗

(k)ν(k)]

= G∗(k)τ∗(k)φ∗

(k+ 1,k)α(k+ 1)+G∗(k)H∗

(k)ν(k)6.18,6.14

= ν(k)


Solution 7.1 (pp. 121): Decomposition of φMφ∗ using P

1. Equation (7.7) on page 117 states that M = P−EφPE∗ψ. Pre and post multiply-

ing this equation by φ and ψ∗, respectively leads to

φMψ∗ 3.41,7.8= φPψ∗

− φPψ∗= (φ+ I)P(ψ∗

+ I)− φPψ∗

= φPψ∗+P+ φP+Pψ∗

− φPψ∗= P+ φP+Pψ∗

and the result follows.2. We have

φMφ∗= (φψ−1

)ψMφ∗ 7.15= (φψ−1

)[ψP+Pφ∗]

7.11= φP+[I+φKH]Pφ∗

= P+ φP+Pφ∗+φKHPE∗

φφ∗

6.13= P+ φP+Pφ∗

+φKDG∗E∗φφ

∗ 6.36= P+ φP+Pφ∗

+φKDK∗φ∗

3. From (4.10) on page 61 it follows that R forms the diagonal part ofφMφ∗, while(7.16) shows that its diagonal part is given by P plus the positive semi-definitediagonal part of φKDK∗φ∗. This establishes that R � P.

C Solutions 439

Solution 7.2 (pp. 125): Expression relating z and z+

1. We have

ε7.25b= [I−HψK]T −Hψ[Pa+b] (C.24)

Therefore

TC.24= [I−HψK]

−1{ε+Hψ[Pa+b]}

7.17,7.12= [I+HφK]ε+Hφ[Pa+b]

(C.25)

Therefore

z7.25a= ψ[KT +Pa+b]

C.25= ψK[I+HφK]ε+ {ψKHφ+ψ}[Pa+b]

7.10,7.12= φKε+ {φ−ψ+ψ}[Pa+b]

from which (7.26) follows.2. Pre-multiplying (7.26) by φ−1 = (I−Eφ) and rearranging terms, we have

z = Eφz+ [Kε+Pa+b]7.3= Eφ(z+Gε)+Pa+b

7.27= Eφz

++Pa+b

3. Now

z+ 7.27

= z+Gε7.26= φ[Kε+Pa+b]+Gε

7.3= φGε+φ[Pa+b]+Gε= φ[Gε+Pa+b]

Solution 7.3 (pp. 125): Expression for α in terms of νFrom (7.25d) we have θ= [I−HψK]∗ν−K∗ψ∗a. Thus, it follows that

α5.23= φ∗

[H∗ θ+a]7.25d= φ∗H∗{

[I−HψK]∗ν−K∗ψ∗

a}

+φ∗a

7.12= ψ∗H∗ν−φ∗H∗K∗ψ∗

a+φ∗a

7.10= ψ∗H∗ν+ψ∗

a = ψ∗[H∗ν+a]

This establishes (7.30).The first equality in (7.31) results from combining (7.25d) and (7.30) as follows:

θ7.25d= [I−HψK]

∗ν−K∗ψ∗

a = ν−K∗ψ∗H∗ν−K∗ψ∗a

7.30= ν−K∗α

The second equality follows from the additional use of (7.3) in the first equality.

440 C Solutions

Solution 7.4 (pp. 127): Relationship between ν, α and a

The first identity in (7.33) follows from

α7.30= ψ∗

[H∗ν+a]7.8= ψ∗

[H∗ν+a]+ [H∗ν+a]

7.8= E∗

ψψ∗[H∗ν+a]+ [H∗ν+a]

7.25d= E∗

ψα+H∗ν+a

7.32e= τ∗α+

+H∗ν+a

The second identity in (7.33) follows by multiplying both sides of the first identityby G∗ and noting that G∗τ∗ = 0 and G∗H∗ = I.

Solution 7.5 (pp. 128): Computing inter-link spatial force f(k)

We have

f5.23= φ[Mα+b]

7.30= φ

[

Mψ∗(H∗ν+a)+b

]

7.15= [φP+Pψ](H∗ν+a)+φb

7.30= φPH∗ν+ φPa+Pα+φb

7.3= φKε+φPa−Pa+Pα+φb

7.26= P[α−a]+z

This establishes the first half of (7.34).From (7.33) we have that α= τ∗α+ +H∗ν+a. It thus, follows that

f7.34= P(α−a)+z

7.33= P[τ∗α+

+H∗ν+a]−Pa+z

6.25,7.32b= P+α+

+PH∗ν+z+

−Gε7.25c= P+α+

+z+

Solution 7.6 (pp. 129): Including gravitational accelerations

1. The proof here is an extension of the proof of Lemma 7.6 on page 123 to includein the additional gravitational term. The Coriolis term expression with gravity

included in is defined by (5.39). and consists of replacing a with a ′ where a ′ �=

a+E∗g. The new version of (7.23) with the gravitational term requires a similarchange to obtain

[I−HψK]Hφ(Mφ∗a′+b)

= (HψP+DK∗φ∗)a

′+Hψb

= (HψP+DK∗φ∗)/1a+(HP+DK∗φ∗

)E∗g+Hψb

7.3= (HψP+DK∗φ∗

)a+D(G∗+K∗φ∗

)E∗g+Hψb

7.3,3.41= (HψP+DK∗φ∗

)a+DG∗(I+ φ∗

)E∗g+Hψb

3.40= (HψP+DK∗φ∗

)a+DG∗φ∗E∗g+Hψb

5.41= (HψP+DK∗φ∗

)a+DG∗E∗g+Hψb

(C.26)

C Solutions 441

When compared with (7.23), (C.26) contains the additional DG∗E∗g gravitationalterm. Updating (7.24) to include this additional term results in the following al-tered expression:

[I−HψK]∗D−1[I−HψK]Hφ(Mφ∗

a+b))

= [I−HψK]∗D−1

[Hψ(Pa+b)]+K∗ψ∗a+ [I−HψK]

∗G∗E∗g

The additional [I−HψK]∗G∗E∗g term also needs to be included in (7.21) result-ing in the desired (7.36) expression.

2. We repeat the steps in the proof of Exercise 7.3, but this time using the expressionfor θ from (7.36) instead of from (7.21). We obtain:

α5.23= φ∗

[H∗ θ+a]7.30,7.36

= ψ∗[H∗ν+a]−φ∗H∗

[I−HψK]∗G∗E∗g

7.12= ψ∗

[H∗ν+a]−ψ∗H∗G∗E∗g =ψ∗ [H∗ (ν−G∗E∗g)

+a]

7.37= ψ∗ [H∗ν+a]

establishing (7.38a).Equation (7.38b) is obtained by simply using (7.25c) in (7.36).For (7.38c) repeat the steps in the proof of (7.31).

3. The proof of (7.39) exactly parallels the proof of Exercise 7.4 on page 127 exceptfor using the expression for α in (7.38a) as a starting point.

4. The proof of (7.40) is similar to that of Exercise 7.5. We have

f5.23= φ

[

M(α+ E∗g)+b] 7.38a

= φ[

M[

ψ∗(H∗ν+a)+ E∗g

]

+b]

7.15=

[

φP+Pψ]

(H∗ν+a)+φb+φME∗g7.38a,7.37

= φPH∗ν+ φPa+Pα+φb+[

φM− φPH∗G∗] E∗g5.41,7.25c

= φKε+φPa−Pa+Pα+φb+[

φMφ∗− φPH∗G∗φ∗]E∗g

7.26,7.16= P[α−a]+z+

[

φP+Pφ∗− φPH∗G∗]E∗g

5.41,7.3= P

[

α+ E∗g−a]

+z+[

φP− φPH∗G∗]E∗g

= P[

α+ E∗g−a]

+z+ φPτ∗E∗g = P[

α+ E∗g−a]

+z

The last step follows from the φPτ∗E∗g = 0 identity which is a consequence ofthe strictly lower-triangular nature of φ. This establishes the first half of (7.40).Now using (7.39) in the above we have

f7.40= P

(

α+ E∗g−a)

+z7.39= P

[

τ∗α++H∗ (ν−G∗E∗g

)

+a+ E∗g−a]

+z

6.25,7.32b= P+

(

α++ E∗g

)

+PH∗ν+z+

−Gε7.25c= P+

(

α++ E∗g

)

+z+

5. (7.41) follows directly from the operator expression in (7.38c) for θ.

442 C Solutions


Solution 8.1 (pp. 147): Root/tip nodes and BWA matrices

1. The first part of (8.20) is a generalized restatement of (8.4) for BWA matrices.The latter half follows from the following:

A−1 · er 8.15

= (I−EA)er8.20= er

2. The proof here is completely analogous to that of the first part with the use of(8.5).


Solution 9.1 (pp. 165): Recursive evaluation of Ax and A∗x

We have that y�= Ax= Ay−x where y= Ax. This implies that

y(k) = y(k)−x(k) =9.5=

∑∀i∈�(k)

A(k, i)y(i) =∑

∀i∈�(k)

A(k, i)[y(i)+x(i)]

Thus, the computational algorithm for y is the one in (9.5) except that the compu-tational step in the loop is now given by the above expression with initial conditiony(0) = 0.

Similarly y�= A

∗x = y− x where y = A∗x. It thus follows that the computa-

tional algorithm for A∗x is the same as in (9.8) except that the computational step

in the loop is now given by

y(k) = A∗(℘(k),k) [y(℘(k))+x(℘(k))]

with initial condition y(n+ 1) = 0.

Solution 9.2 (pp. 180): Determinant of the mass matrix

1. We haveI−HAK

9.37= I−HAEAG

8.19= I−HAG

For a canonical tree, A is strictly lower triangular, while H and G are block-diagonal. Hence, [I +HAK] is lower triangular with identity blocks along thediagonal. Moreover, from (9.50) we know that [I −HψK] is its inverse. From

C Solutions 443

matrix theory, we know that the inverse of a lower-triangular matrix is alsolower-triangular, and that the diagonal elements are inverses of each other. Itthus, follows that for a canonical tree, [I−HψK] is also lower-triangular withidentity blocks along its diagonal.

2. For canonical trees, the above part established that [I+HAK] is a lower-triangular matrix with identity matrices along the diagonal. Since, the deter-minant of a lower-triangular matrix is the product of the determinants of theblock elements along its diagonal, it follows that (9.52) holds for canonical trees.Since all tree can be converted into canonical trees by a simple renumberingof the bodies, there exists a permutation matrix which transforms the Newton–Euler factors for a tree into the corresponding factors for a canonical versionof the tree. Since permutation matrices are orthogonal, their determinants are 1,and hence, the determinant of the canonical and non-canonical versions of theNewton–Euler factors are equal to each other and are both 1. This establishes(9.52).

3. For (9.53), we have

det {M}9.49= det

{[I+HAK]D [I+HAK]

∗}= det {[I+HAK]}det {D}det

{[I+HAK]

∗}9.52= det {D} =

∏k=1,n

det{D(k)}


Solution 10.1 (pp. 197): Υ(k) for a micro/macro manipulator systemFrom (10.16) we have the following general expression for Υ(k):

Υ(k) =∑

∀i: i�kψ∗

(i,k)H∗(i)D−1

(i)H(i)ψ(i,k) (C.27)

Thus,

Υ(1) =∑

∀i: i�1

ψ∗(i,1)H∗

(i)D−1(i)H(i)ψ(i,1)

=∑

∀i: ℘(S) i�1

ψ∗(i,1)H∗

(i)D−1(i)H(i)ψ(i,1)

+∑

∀i: i�℘(S)

ψ∗(i,1)H∗

(i)D−1(i)H(i)ψ(i,1)

444 C Solutions

8.17,C.27= ΥS(1)+ψ∗

(℘(S),1)

{ ∑∀i: i ℘(S)

ψ∗(i,℘(S))H∗

(i)∗

D−1(i)H(i)ψ(i,℘(S))

}ψ∗

(℘(S),1)

C.27= ΥS(1)+ψ∗

(℘(S),1)Υ(℘(S))ψ∗(℘(S),1)

The above steps have used the fact that D(i) does not depend on the gener-alized coordinates and other properties of the bodies inboard of the ith bodyin including ΥS(1) in the above expressions. This establishes (10.23). (10.24)is a direct consequence of (10.23) and the positive semi-definite nature ofψ∗(P(℘(S),1)Υ(℘(S))ψ(P(℘(S),1).

Solution 10.2 (pp. 199): Computation of the mass matrix inverse

1. Expanding out the factorized form of the mass matrix inverse, it follows that

M−1 9.51= D−1

−K∗ψ∗H∗D−1−D−1HψK+K∗ΩK

10.13= D−1

−K∗ψ∗H∗D−1−D−1HψK+K∗ [Υ+ ψ∗Υ+Υψ+R

]

K

Therefore,

M−1 9.51= D−1

−K∗ψ∗H∗D−1−D−1HψK

+K∗ [Υ+ψ∗E∗ψΥ+ΥEψψ+R

]

K

= (D−1+G∗ΥG)−K∗ψ∗

(H∗D−1−τ∗ΥG)

− (D−1H−G∗Υτ)ψK+K∗RK

10.30= L−K∗ψ∗U−UψK+K∗RK

The last equality used the following

D−1H−G∗Υτ= D−1H−G∗Υ+G∗Υτ= D−1H−G∗Υ+G∗ΥGH

= LH−G∗Υ 10.30= U

This establishes the decomposition of M−1.2. For a serial-chain system, R = 0, and hence (10.29) reduces to (10.31). Also,

for a serial-chain system E∗φΥEφ is block-diagonal, and hence, Υ= Υ+ follows

from (10.20). Since the component terms of L in (10.30) are block-diagonal, sois L. Similarly, U is block-diagonal as well. The product ψK is strictly lower-triangular, establishing that (10.31) is a decomposition into block-diagonal,strictly upper-triangular and strictly lower-triangular terms.

C Solutions 445

Algorithm 10.4 is a recursive implementation of the above expressions for theelements of M−1.

Solution 10.3 (pp. 207): Ground connected equations of motionIn Lemma 10.6, assume that system B is the inertial frame. Since the inertial frameis immovable, we have

ΛB = 0 and CBos = 0

Substituting these into (10.48) and keeping just the upper part results in (10.53).


Solution 11.1 (pp. 214): Mapping between T and θ for closed-chainsystems

This result is obtained by setting combining together (11.9a)-11.9c and (11.8).

Solution 11.2 (pp. 215): Torque minimization using squeeze forcesThe squeeze force has the parametric form Tsq =G∗

cλ for some λ. The norm of theoverall generalized force is given by

‖T‖2W = ‖Tmv+Tsq‖2

W = ‖Tmv+G∗cλ‖2

W = (Tmv+G∗cλ)

∗W(Tmv+G∗cλ)

The gradient of the above with respect to λ must be zero for the minimum normsolution. Taking the gradient and setting it to zero yields,

0 =GcW(Tmv+G∗cλ) ⇒ λ= −(GcWG

∗c)

−1GcW Tmv (C.28)

Thus X = GcW minimizes the nrom. Since the squeeze force is Tsq = G∗cλ, the

minimum norm T value is

Tmv+G∗cλ

11.14,C.28= Tmv−PmsTmv

11.15= (I−Pms)(I−Pms)T = (I−Pms)T

The last step used the projection matrix property of (I−Pms).

Solution 11.3 (pp. 218): Expression for YC with loop constraintsThis result is obtained by combining the expressions in and (11.20) and (11.21).

Solution 11.4 (pp. 223): Explicit expression for PThe result follows by directly using the expression for P in (11.37).

446 C Solutions

Solution 11.5 (pp. 224): Transformed and partitioned augmenteddynamics

Substituting θ = Pθ into (11.31) transforms it as follows:(

M G∗c

Gc 0

)[

Pθ

−λ

]

=

[

T −C

U

]

(C.29)

Pre-multiplying the top half of both sides of (C.29) with P∗ leads to(

M G∗

G 0

)[

θ

−λ

]

11.37,11.41=

[

P∗(T −C)

U

]

11.41=

[

T

U

]

(C.30)

Applying the partitioned structure in G= [Gr,0] from (11.37) to (C.30) leads to thepartitioned equations in (11.42).

Solution 11.6 (pp. 224): Reduction of augmented dynamicsWe begin by rearranging the blocks in the transformed equations of motion in(11.42) to obtain:

⎛

⎜

⎝

M11 G∗r M12

Gr 0 0

M21 0 M22

⎞

⎟

⎠

⎡

⎢

⎣

θ1

−λ

θ2

⎤

⎥

⎦=

⎡

⎢

⎣

T1

U

T2

⎤

⎥

⎦(C.31)

Using (A.10) to solve the above equation, we obtain

θ2 =

⎧⎨⎩M22 −[M21, 0]

(

M11 G∗r

Gr 0

)−1[

M12

0

]

⎫⎬⎭

−1

(C.32)

.

⎛

⎝T2 −[M21, 0]

(

M11 G∗r

Gr 0

)−1[

T1

U

]

⎞

⎠

Additionally, from (A.11) we have

(

M11 G∗r

Gr 0

)−1

=

(

0 G−1r

G−∗r −G−∗

r M−111 G

−1r

)

(C.33)

Substituting this into (C.32) and expanding out the matrix products we see that theequation simplifies to

θ2 = M−122

(

T2 −M21G−1r U

)

or M22 θ2 = T2 −M21G−1r U (C.34)

C Solutions 447

Solution 11.7 (pp. 224): Transformed projected dynamicsUsing (11.39) in (11.36) we see that

Mr11.39,11.36

= X∗cP

∗MPXc11.41= X∗

cMXc11.38= M22 (C.35)

Thus, the projected dynamics equations of motion in (11.36) becomes

M22 θr11.43= X∗

c(T −C−Mθp)11.39,C.30,11.42

=(

T2 −X∗cMP−1θp

)

(C.36)

Note however, that θp defined by

θp�= P

[

G−1r U

0

]

∈RN (C.37)

satisfies the particular solution condition, (11.35), since

Gc θpC.37= GcP

[

G−1r U

0

]

11.37= [Gr, 0]

[

G−1r U

0

]

= U

Substituting (C.37) in (C.36) leads to

M22θr =

(

T2 −X∗cM

[

G−1r U

0

])

11.38=

(

T2 −M21G−1r U

)

(C.38)

With θr ≡ θ2, this last expression agrees with (11.43) and establishes the result.


Solution 12.1 (pp. 238): Simplification of RG(k)

The expression for RG(k) in (12.18) and the recursion in (12.19) for RGl(k) fol-lows from substituting the expression forΦG(k+ 1,k) from (12.4) into (12.14) andexpanding out the partitioned products.A similar process leads the expressions for Xl(k+ 1), Xl(j+ 1) and MG(k,k).

Solution 12.2 (pp. 242): Structure of PG(k) and related quantitiesThe partitioned structure and related recursive algorithms follow by starting withthe expressions in (12.21) and (12.25) and using the partitioned structure ofΦG(k+ 1,k) to explicitly compute the products.

448 C Solutions


Solution 13.1 (pp. 269): Additional forward dynamics simplificationsFrom (13.69), Dm(k) = HMfl(k)

[

Afl(k)Γfl(k)A∗fl(k)+Mfl(k)

]

H∗Mfl(k).

Applying the (A.19) matrix identity,

[A+BCB∗]−1= A−1

−A−1B[C−1+B∗A−1B]

−1B∗A−1

to the expression for Dm(k) with

A =HMfl(k)Mfl(k)H∗Mfl(k), B=HMfl(k)Afl(k), C= Γfl(k)

establishes (13.70).


Solution 14.1 (pp. 274): Non-tree path-induced sub-graphs

1. Since a directed cycle containing an edge, represents a directed path connectingthe node pair for the edge, the path (and the cycle) must belong to S since it ispath-induced.

2. Similarly, all paths connecting a pair of nodes in S must be in the sub-graphsince it is path-induced, and the result follows.

Solution 14.2 (pp. 278): Mass matrix invariance of the outer sub-systemThe expressions in (14.6) and (14.7) are obtained by directly evaluating M =

HAMA∗H∗ using the following component partitioned expressions from (14.4)

and (14.5):

H=

(

HS 00 HP

)

, A =

(

AS 0APESAS AP

)

, M =

(

MS 00 MP

)

Solution 14.3 (pp. 292): Mass matrix invariance with aggregationFirst, we have

HA14.18,14.20

= (HaJ−1a )(JaAa) =HaAa (C.39)

Using this directly establishes the M equalities in (14.24).

C Solutions 449

Moreover

HA(MA∗a+b)

C.39= HaAa(MA

∗a+b)

14.18= HaAa(MA

∗aJ

−∗a a+b)

14.21= HaAa(MA

∗aa+b)

This establishes the C equalities in (14.24).


Solution 15.1 (pp. 310): Expression for XS

With Z�= Y−1

1 Y2,

dZdt

=dY−1

1

dtY2 +Y−1

1dY2

dtA.28= −Y−1

1dY1

dtY−1

1 Y2 +Y−11

dY2

dt

= Y−11

[

dY2

dt−

dY1

dtZ

]

Thus,

XS =

[

−Z

0

]

=

[

Y−11

[

Y1Z− Y2]

0

]

=

[

−Y−11 YXS

0

]

This establishes the first half of (15.27).Using this, we have

XS θRS =

[

−Y−11 YXS θRS

0

]

=

[

−Y−11 Y θS

0

]

This establishes the second half of (15.27).


Solution 16.1 (pp. 320): Alternate expression for SaaWe have

φU∗⊥MU⊥φ∗ 10.36

= φMU⊥φ∗ 10.39a= φM(ψ∗

−ΩP) = φMψ∗−φU

∗P9.41,10.38a

= φP+Pψ∗−(φ−ψ+PΩ)P = (ψ−PΩ)P+Pψ∗

Pre- and post-multiplying the above with Ha and H∗a and comparing with the ex-

pression for Saa in (16.11c) establishes the lemma.

450 C Solutions

Solution 16.2 (pp. 325): Expression for the generalized JacobianIgnoring all the velocity dependent terms, we have

αnd16.18= Jθ = Ja θa+Jpθp

16.20= Ja θa+JpJD θa = (Ja+JpJD)θa

Solution 16.3 (pp. 330): The disturbance Jacobian for a free-flyingsystem

Equation (4.18) on page 63 showed that the center of mass spatial velocity, VC isgiven by the following relationship when the base-body generalized forces are zero:

VC = V(n)+R−1(n)

n−1∑k=1

φ(n,k)R(k)H∗(k)θ(k) (C.40)

For the case when the system has zero spatial momentum, VC = 0 and so we canre-express (C.40) as:

V(n) = −R−1(n)

n−1∑k=1


The expression for the JD disturbance Jacobian is obtained by converting (16.28)into a matrix form.


Solution 17.1 (pp. 333): Weight matrices for the dual modelIn the dual model,, the kth link is the parent of the (k+1)th link. Thus the velocityrecursion in (3.19b) can be re-expressed as:

V(k) = φ∗(k− 1,k)V(k− 1)−φ∗

(k− 1,k)H∗(k− 1)θ(k− 1)

Thus φ∗(k− 1,k) matrices are the weight matrices in the dual model.

Solution 17.2 (pp. 335): Dual articulated body inertia properties

1. Using (17.4) we have

S(k)H∗(k) = S+

(k)[

I+H∗(k)G∗

dl(k)]

H∗(k)

= S+(k)

[

I−H∗(k)D−1

dl (k)H(k)S+(k)

]

H∗(k) = 0


C Solutions 451

2. Again using (17.4) we have

τdl(k)τ(k) = τdl(k)+Gdl(k)H(k)G(k)H(k) = τdl(k)+Gdl(k)H(k) = 0

This establishes the first expression in (17.6).The latter expression has an analogous proof.

Solution 17.3 (pp. 347): Relationship between φ∗S and φ∗

R

φ∗R(φ∗

S)−1

= φ∗R(I−E∗

φS) = (φ∗

R− I)−φ∗RE∗φR

E∗φS

17.22,17.28a= (φ∗

R− I)−φ∗RSS

∗ 17.27= φ∗

Reke∗k− I

Post-multiplying the above with φ∗S establishes (17.36).

Solution 17.4 (pp. 347): The inverse transformation JGR

Observe that

JGR θR17.38=

(

S∗RG+ eGne∗nφ

∗RH

∗R

)

θR17.29,17.30

=

n−1∑j=k

eGj θR(j+ 1)+ eGne∗nVS

=

n−1∑j=k

eGj θR(j+ 1)+ eGnVS(n)17.18= θS

This establishes (17.38).Now,

JGR ∗JRG17.28c,17.38

= (S∗RG+ eGne∗nφ

∗RH

∗R)(SRG+ eRke∗kφ

∗SH

∗S)

17.37a= S

∗RGSRG+ eGne∗nφ

∗RH

∗R(SRG+ eRke∗kφ

∗SH

∗S)

(C.41)

However,

φ∗RH

∗R

17.28b= φ∗

R(−E∗φRH∗

SS∗RG+ eke∗Rk)

⇒ φ∗RH

∗RSRG

17.37a= φ∗

R(−E∗φRH∗

SS∗RGSRG)

17.37b,17.37c= −φ∗

RH∗S

(C.42)

Also,

φ∗RH

∗ReRke∗k

17.28b= φ∗

R(−E∗φRH∗

SS∗RG+ eke∗Rk)eRke∗k

17.37a= φ∗

Reke∗k (C.43)

452 C Solutions

Therefore

JGR ∗JRGC.41,C.42,C.43

= S∗RGSRG+ eGne∗n(−φ∗

RH∗S +φ∗

Reke∗kφ∗SH

∗S)

17.35= S

∗RGSRG+ eGne∗n(−φ∗

R+ φ∗R+φ∗

S)H∗S

= S∗RGSRG+ eGne∗nφ

∗SH

∗S

17.37e= S

∗RGSRG+ eGne∗nH

∗S

17.37d= S

∗RGSRG+ eGne∗Gn

17.37e= I


Solution 17.5 (pp. 348): Transformed mass matrix

1. We haveφ∗RH

∗RJRG

17.32= φ∗

R(−E∗φR

+ eke∗kφ∗S)H∗

S)

17.35= (−φ∗

R+ φ∗R+φ∗

S)H∗S = φ∗

SH∗S

establishing (17.43).2. The θ= J

RGθ directly from the use of (17.28c) and the definition of θ in (17.40).

For the latter equality, we have

φ∗H∗JRG

17.40,17.41,17.44=

(

φ∗CH

∗C φ∗

CB∗Sφ

∗RH

∗RJRG

0 φRH∗RJRG

)

17.43=

(

φ∗CH

∗C φ∗

CB∗Sφ

∗SH

∗S

0 φ∗SH

∗S

)

14.4,17.17= φ∗H∗

This establishes (17.44).3. We have

M =HφMφ∗H∗ 17.44= J

∗RGHφMφ∗H∗

JRG

17.42= J

∗RG

MJRG

This establishes (17.45).4. Now,

Ke =12θ∗Mθ

17.45=

12θ∗J∗RG

MJRGθ

17.44=

12θ∗Mθ

establishing (17.46)5. φ is an SPO operator andH and M are both block-diagonal. Also,H is full rank

because H and HR are both full rank. Thus, the SKO model requirements aresatisfied.

C Solutions 453


Solution 18.1 (pp. 356): Identities for ˜V

At the component level,

V+(k)

3.15= φ∗

(℘(k),k)V(℘(k)) ⇒ ˜V+(k)φ∗

(℘(k),k)1.35= φ∗

(℘(k),k) ˜V(℘(k))

The above relationship establishes the component level equivalence of the elementsof the operator expressions on the left and right of (18.9a).Equation (18.9b) then follows from using the fact that V = V+ +ΔV in (18.9a).Equation (18.9c) follows from pre- and post-multiplying (18.9b) by φ∗ andsimplifying.Equation (18.9d) is simply a transposed version of (18.9c).

Solution 18.2 (pp. 358): Time derivative of Hφ

dHφdt

= Hφ+Hφ18.12,18.5

= −H ˜VωSφ+H

[

φΔVφ+ ˜Vωφ−φ˜Vω]

18.5= H ˜ΔωV φ+HφΔVφ−Hφ˜Vω

=H ˜ΔωV φ+Hφ

[

˜ΔωV + Δv

V

]

φ−Hφ˜Vω

=Hφ ˜ΔωV φ+HφΔv

Vφ−Hφ˜Vω =Hφ[

˜ΔωV φ+EφΔv

Vφ− ˜Vω]

This establishes the result.

Solution 18.3 (pp. 358): Operator expression for the aI Coriolisacceleration

1.

φ∗aI

8.43=

dIφ∗H∗

dtθ

18.13= [ ˜Vω −φ∗

˜ΔωV −φ∗˜ΔvV E∗

φ]φ∗H∗ θ

18.13= [ ˜Vω −φ∗

˜ΔωV −φ∗˜ΔvV E∗

φ]V

(C.44)

Hence,aI

C.44= φ−∗

[ ˜Vω −φ∗˜ΔωV −φ∗

˜ΔvV E∗φ]V

= [φ−∗˜Vω − ˜ΔωV − ˜ΔvV E∗

φ]V

= − ˜ΔωV V+[I−Eφ∗] ˜VωV− ˜ΔvV V+

= − ˜ΔωV V+ ˜VωV−E∗φ˜VωV− ˜ΔvV V+

18.5= ˜VωS V−E∗

φ˜VωV− ˜ΔvV V+

(C.45)


454 C Solutions

2. The kth element of aI from (C.45) is given by:

aI(k) =

[

ω(℘(k))ω(k)

ω(℘(k))v(k)

]

−φ∗(℘(k),k)

[

0ω(℘(k))v(℘(k))

]

−

[

0˜Δv(k)ω(℘(k))

]

=

[

−ω(k)ω(℘(k))

ω(℘(k))[

v(k)−v(℘(k))+Δv(k)]

]

=

[

−ω(k)[ω(k)−Δω(k)]

ω(℘(k))[

v(k)−v(℘(k))+Δv(k)]

]

The further use of ℘(k) = k+ 1 establishes the result

Solution 18.4 (pp. 359): Time derivative of φ(k+ 1,k) with Ok �= Bk

1. Equation (18.17) follows from (1.37) when applied to the Bk and Ok frame pairon the kth rigid link.

2. Equation (18.18a) follows directly from Exercise 1.8 on page 13.Equation (18.18b) follows from differentiating (18.15).Equation (18.18c) and (18.18d) follow from further manipulation and rearrange-ment of the earlier expressions.

Solution 18.5 (pp. 360): Operator time derivatives with Ok �= Bk

1. (18.21a) follows from the use of (18.17) in its operator definition in (18.20).For (18.21b), we have

dΔ−1B/O

dt18.21a,A.28

= −Δ−1B/O

dΔB/O

dtΔ−1

B/O

18.21a= −Δ−1

B/OVB/OΔ

−1B/O

1.36= −VB/O

In the last equality we have used the fact that VB/O only contains non-zero linearvelocity values together with (1.36) in:

VB/O = VB/OΔ−1B/O

= ΔB/O VB/O (C.46)

C Solutions 455

2. We have

dEφB

dt5.29=

dΔB/OEφΔ−1B/O

dt18.21a,18.21b

= ΔB/O

dEφ

dtΔ−1

B/O+ VB/OEφΔ

−1B/O

−ΔB/OEφVB/O

C.46= ΔB/O

dEφ

dtΔ−1

B/O+ VB/OΔB/OEφΔ

−1B/O

−ΔB/OEφΔ−1B/O

VB/O

5.29= ΔB/O

dEφ

dtΔ−1

B/O+ VB/OEφB

−EφBVB/O

This establishes (18.22a).For (18.22b),

dφB

dtA.28= φB

d(I−EφB)

dtφB

18.22a= φB

(

ΔB/O

dEφ

dtΔ−1

B/O+ VB/OEφB

−EφBVB/O

)

φB

5.30= ΔB/Oφ

dEφ

dtφΔ−1

B/O+φB VB/OφB − φB VB/OφB

5.30= −ΔB/Oφ

d(I−Eφ)

dtφΔ−1

B/O−φB VB/O + VB/OφB

A.28= −ΔB/O

dφdtΔ−1

B/O−φB VB/O + VB/OφB

For (18.22c), we have

dHB

dt5.31=

dHΔ−1B/O

dt18.21b= HΔ−1

B/O−HVB/O

C.46= HΔ−1

B/O−HΔ−1

B/OVB/O

5.31= HΔ−1

B/O−HB VB/O

Solution 18.6 (pp. 361): Alternative derivation of M expression

1. We have

dφMφ∗

dt= φMφ∗

+φMφ∗+φMφ

∗

18.12=

[

φΔVφ+ ˜Vωφ−φ˜Vω]

Mφ∗+φ

[

˜VωM−M˜Vω]

φ∗

+φM[

−φ∗˜ΔV φ

∗−φ∗

˜Vω + ˜Vωφ∗]

=

[

φΔV + ˜Vω]

φMφ∗−φMφ∗

[

˜ΔV φ∗+ ˜Vω

]


456 C Solutions

2. For M, we have

M = HφMφ∗H∗+H

dφMφ∗

dtH∗

+HφMφ∗ H∗

18.12,18.24= −H ˜Vω

SφMφ∗H∗

+HφMφ∗˜Vω

SH∗

+H{[φΔV + ˜Vω

]

φMφ∗−φMφ∗

[

˜ΔV φ∗+ ˜Vω

]}H∗

18.5= H

{[φΔV + ˜ΔωV

]

φMφ∗−φMφ∗

[

˜ΔV φ∗+ ˜ΔωV

]}H∗

=Hφ

{[

EφΔV +(I−Eφ) ˜ΔωV

]

φM

−Mφ∗[

˜ΔV E∗φ+ ˜ΔωV (I−E∗

φ)

]

}φ∗H∗

1.22= Hφ

{[EφΔ

v

V + ˜ΔωV

]

φM−Mφ∗[

˜ΔvV E∗φ+ ˜ΔωV

]}φ∗H∗


Solution 18.7 (pp. 363): Sensitivities of φ(℘(k),k), H(k) and M(k)

The expressions in (18.29) follow directly from applying (18.26) and (18.28) to thetime derivative expressions in (18.10), together with the expressions in (18.28).

Solution 18.8 (pp. 364): Sensitivity of HφEquation (18.34) follows from applying (18.26) and (18.31) to the time derivativeexpressions in (18.13).

Solution 18.9 (pp. 368): Equivalence of Lagrangian and Newton–Eulerequations of motion

1. From (2.2) we see that the bI stacked vector can be expressed as

bI =[

VM−MV]

V

Also, we have seen in (C.44) that

φ∗aI =

[

˜Vω −φ∗˜ΔωV −φ∗

˜ΔvV E∗φ

]

V

Using these in (18.39) leads to

C(θ, θ) =Hφ

[

VM−MV+M(

˜Vω −φ∗˜ΔωV −φ∗

˜ΔvV E∗φ

)]

V

1.22= Hφ

[

VM−MVv−Mφ∗

(

˜ΔωV + ˜ΔvV E∗φ

)]

V

C Solutions 457

Noting that VvV = 0 helps simplify the above expression and leads to the

expression in (18.41). This establishes the first part of this exercise.2. We have

M(θ)θ−12∂[θ

∗M(θ)θ]

∂θ18.23,18.38b

= Hφ

[

V−

(

EφΔv

V + ˜ΔωV

)

φ

]

MV

+Hφ[(

˜ΔωV +EφΔv

V

)

φM−Mφ∗(


)]

V

=Hφ

[

VM−Mφ∗(


)]

V

This is indeed the expression in (18.41) and establishes the second part of thisexercise.

Solution 18.10 (pp. 373): Physical interpretation of λIn general

P(k) =dIP(k)

dt=

dO

+kP(k)

dt+ ˜Vω(℘(k))P(k)−P(k)˜Vω(℘(k))

Comparing this expression with that of P(k) in (18.50) establishes the equality forλ(k) in (18.51). Also,

λ(k)18.51=

dO

+kP(k)

dt=

dOkP(k)

dt+ ˜ΔωV (k)P(k)−P(k) ˜ΔωV (k)

Comparing this expression with that for λ(k) in (18.48) establishes the equality forP(k) in (18.51). On a similar note,

P+

(k) =dIP

+(k)

dt=

dO

+kP+(k)

dt+ ˜Vω(℘(k))P+

(k)−P+(k) ˜Vω(℘(k))

Comparing this expression with that for P+

(k) in (18.50) establishes the equalityfor P+(k) in (18.51).

Solution 18.11 (pp. 375): Time derivative of (I+HφK)

We have

[I+HφK] = [I+HφG] = [I−HG+HφG] =HφG (C.47)

458 C Solutions

Thus,

d[I+HφK]

dt=

dHφdt

G+HφG

18.13,18.43c= Hφ

[(

˜ΔωV φ− ˜Vω +EφΔv

Vφ

)

G+τλH∗D−1+ ˜VωS G

]

18.5= Hφ

[(

˜ΔωV φ− ˜ΔωV +EφΔv

Vφ

)

G+τλH∗D−1]

=Hφ[(

˜ΔωV φ+EφΔv

Vφ)

G+τλH∗D−1]

=Hφ

[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

Solution 18.12 (pp. 376): Time derivative of [I+HφK]D

We have

d[I+HφK]D

dt=

d[I+HφK]

dtD+[I+HφK]

dD

dt18.61,18.43a,C.47

= Hφ

[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

D+HφGHλH∗

=Hφ

[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

D+HφτλH∗

=Hφ[

˜ΔωV φK+EφΔv

VφG+ λH∗D−1]

D

=Hφ

[

˜ΔωV φP+EφΔv

VφP+ λ

]

H∗

Solution 18.13 (pp. 376): Time derivative of [I−HψK]

Since [I−HψK]−1 = [I+HφK] it follows from (A.28) on page 402 that

d[I−HψK]

dt= −[I+HφK]

d[I−HψK]

dt[I−HψK]

18.61= −[I+HφK]Hφ

[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

[I−HψK]

9.45= −Hψ

[

˜ΔωV φK+EφΔv

VφG+τλH∗D−1]

[I−HψK]

This establishes the result.

Solution 18.14 (pp. 376): Sensitivities of [I+HφK] and [I−HψK]

The expressions are obtained by starting with the time derivative expressions in(18.61) and (18.63) and using the standard process for converting them into sensi-tivity expressions.

C Solutions 459

Solution 18.15 (pp. 377): Sensitivity of log {det {M}}

1. From (9.53) on page 180, we have

det {M} =

n∏k=1

det {D(k)}

Hence,

log {det {M}} =

n∑k=1

log {det {D(k)}} (C.48)

Differentiating with respect to t we have

dlog {det{M}}

dtC.48=

n∑k=1

d log {det{D(k)}}

dtA.29=

n∑k=1

D−1(k)D(k)

= Trace{D−1D

} 18.43a= Trace

{D−1HλH∗}

To establish the first expression in (18.66), differentiate both sides by θi and use(18.53). Use the component level expressions in (18.55) to establish the latterhalf of (18.66).

2. For (18.68), we have

2Trace {PΩHω=i }

A.22= 2Trace {Hω

=iPΩ}

A.22= Trace {Hω

=iPΩ}+ Trace{[Hω

=iPΩ]∗}

A.22,A.21= Trace {Hω

=iPΩ}− Trace{ΩPHω=i }

= Trace {Hω=iPΩ}− Trace{PHω

=iΩ}

= Trace {[Hω=iP−PHω

=i ]Ω}

= Trace {[Hω=iP−PHω

=i ]Ω}

18.54g= Trace

{[λθi −EψλθiE

∗ψ

]

Ω}

10.12= Trace

{[λθi −EψλθiE

∗ψ

]

ψ∗H∗D−1Hψ}

= Trace{D−1Hψ

[

λθi −EψλθiE∗ψ

]

ψ∗H∗}= Trace

{D−1H

[

ψλθiψ∗− ψλθiψ

∗]H∗}= Trace

{D−1H

[

(I+ ψ)λθi(I+ ψ∗)− ψλθiψ

∗]H∗}= Trace

{D−1H

[

λθi + ψλθi + λθiψ∗]H∗}

= Trace{D−1HλθiH

∗}

The last equality above uses the zero block-diagonal elements property of ψ. Thelast expression is the same as (18.66), and establishes the equivalency of (18.66)and (18.67).

460 C Solutions

3. For (18.68), we have

d log {det {M}}

dtA.29= Trace

{M−1M

}9.51,18.23

= Trace{

[I−HψK]∗D−1

[I−HψK]Hφ·[

( ˜ΔωV +EφΔv

V )φM−Mφ∗( ˜ΔvV E∗

φ+ ˜ΔωV )

]

φ∗H∗}

9.45= Trace{D−1Hψ

[

( ˜ΔωV +EφΔv

V )φM

−Mφ∗( ˜ΔvV E∗

φ+ ˜ΔωV )

]

ψ∗H∗}

10.12= Trace

{[( ˜ΔωV +EφΔ

v

V )φM−Mφ∗( ˜ΔvV E∗

φ+ ˜ΔωV )

]

Ω}

= 2Trace{[

( ˜ΔωV +EφΔv

V )φMΩ

]}

= 2Trace{˜ΔωV φMΩ

}+ 2Trace

{EφΔ

v

VφMΩ

}10.38a= 2Trace

{˜ΔωV [φ−ψ+PΩ

}+ 2Trace

{EφΔ

v

VφMΩ}

10.38a= 2Trace

{˜ΔωV PΩ

}+ 2Trace

{EφΔ

v

VφMΩ

}

The last equality uses the zero diagonal elements property of φ−ψ.


Solution 19.1 (pp. 387): Time derivative of D12

We have

D =dD

12 ·D 1

2

dt=

dD12

dtD

12 +D

12

dD12

dt

For a system with 1 degree of freedom joints, the diagonal block elements of D12

are in fact scalar values, so that D1/2 and dD1/2

dt commute. Thus, the above can betransformed into

dD12

dt=

12D− 1

2 D18.43a=

12D− 1

2HλH∗

Solution 19.2 (pp. 391): Non-working C(θ,η) Coriolis Vector

η∗C(θ,η)19.29= η∗D− 1

2Hψ

[

VM−12

(

˜ΔωV P+P ˜ΔωV

+Eφ

{Δv

V P++P+

˜ΔvV


ψ

)]

V

C Solutions 461

Observe thatη∗D− 1

2Hψ19.23= θ

∗[I+HφK]Hψ

9.45= θ

∗Hφ

Thus,

η∗C(θ,η) = θ∗Hφ

[

VM−12

(

˜ΔωV P+P ˜ΔωV

+Eφ

{Δv

V P++P+

˜ΔvV


ψ

)]

V

9.1= V∗

[

VM−12

(

˜ΔωV P+P ˜ΔωV

+Eφ

{Δv

V P++P+

˜ΔvV


ψ

)]

V

1.26= −

12V∗

[

˜ΔωV P+P ˜ΔωV

+Eφ

{Δv

V P++P+

˜ΔvV


ψ

]

V

Since the matrix expression in the middle is skew–symmetric, the expression evalu-ates to zero.

Solution 19.3 (pp. 392): Expression for �

It follows from �−1= m and (A.28) on page 402 that

�A.28= −�m�

19.25= −D− 1

2 [I−HψK]Hφ

[

˜ΔωV φK

+EφΔv


]

D12 D− 1

2 [I−HψK]

9.45= −D− 1

2Hψ

[

˜ΔωV φK+EφΔv


]

[I−HψK]

Solution 19.4 (pp. 392): Sensitivity of Innovations factorsThe expressions are obtained by starting with the time derivative expressions in(19.25) and (19.33) and using the standard process for converting them into sensi-tivity expressions.

462 C Solutions

Solutions for Appendix A

Solution A.1 (pp. 397): Norm of sWe have

‖sx‖2= −x∗ s sx A.1

= −x∗[

ss∗ −‖s‖2I]

x= ‖s‖2‖x‖2−(x∗s)2

Clearly the above achieves a maximum of ‖s‖2‖x‖2 when x∗s = 0. Hence, using(A.2) it follows that ‖ s‖ = ‖s‖.

Solution A.2 (pp. 401): Product gradient and chain rules

1. The (k, j) element of ∇x [f(x)g(x)] is

∇x[

f(x)g(x)]

(k, j) =∂fk ·g∂xj

=∂fk

∂xj·g+ fk · ∂g

∂xj(C.49)

On the other hand, the (k, j) element of ∇xf(x) ·g(x)+f(x) · .∇xg(x), the right-hand side of (A.25), is given by:

[∇xf(x) ·g(x)+ f(x) · .∇xg(x)]

(k, j) =∂fk

∂xj·g+ fk

∂g

∂xj

This is identical to the expression in (C.49). Thus, the component-level values onboth sides of (A.25) agree and establish (A.25).

2. The (k, j) element of ∇xf(x) is

[∇xf(x)]

(k, j) =∂fk

∂xj=

n∑i=1

∂fk

∂yi· ∂yi∂xj

(C.50)

On the other hand, the (k, j) element of ∇yf(y) ·∇xy(x), the right-hand side of(A.26), is given by:

[∇yf(y) ·∇xy(x)]

(k, j) =

n∑i=1

[∇yf(y)]

(k, i) · [∇xy(x)]

(i, j) =

n∑i=1

∂fk

∂yi· ∂yi∂xj

This is identical to the expression in (C.50). Thus, the component-level values onboth sides of (A.26) agree and establish (A.26).

C Solutions 463

Solutions for Appendix B

Solution B.1 (pp. 403): Time derivative of a rotation matrixSince rotation matrices are orthogonal, RR∗ = I. Differentiating this with respectto time yields

RR∗+RR

∗= 0 ⇒ RR

∗= −[RR

∗]∗

This implies that RR∗ is skew-symmetric, i.e., there exists a 3-vectorw, such that,RR∗ = w. This establishes the result.

Solution B.2 (pp. 405): Derivation of the Euler–Rodrigues formula

1. While this can be established by direct verification using an arbitrary vector s weuse alternative derivation. Thus

s3 A.1= s

(

ss∗ −‖s‖2I)

= −σ2 s (C.51)

We know that the characteristic polynomial of s is a polynomial of order 3 and,that it is the unique polynomial of order 3 that s satisfies as well. It followstherefore, from (C.51) that the characteristic polynomial of s is (λ3 +σ2λ).

2. We have that

RB(n,θ)B.4= exp[ ñθ] = I+ nθ+

ñ2θ2

2!+

ñ3θ3

3!+ . . .

C.51= I+

∞∑k=0

(−1)kθ2k+1

(2k+ 1)!ñ+

∞∑k=0

(−1)kθ2k+2

(2k+ 2)!ñ2

= I+ sin(θ)ñ+[

1 − cos(θ)]

ñ2

A.1= cos(θ)I3 +

[

1 − cos(θ)]

nn∗+ sin(θ)ñ

Solution B.3 (pp. 405): Trace and characteristic polynomialof a rotation matrix

1. The result follows by applying the trace operation to (B.6) on page 405 andnoting that Trace {I} = 3, Trace {nn∗} = 1, and Trace { ñ} = 0.

2. Since R is a 3×3 matrix, its characteristic polynomial is of the form λ3 +aλ2 +

bλ+ c for some constants a, b and c. From matrix theory we know that −a

is equal to the trace γ, and −c is the determinant 1 of the matrix. That is thepolynomial is of the form λ3 −γλ2 +bλ− 1. Furthermore, we know that 1 is aneigen-value of R and hence, must satisfy the characteristic polynomial. Usingthis fact implies that b = γ, establishing the result.

464 C Solutions

3. Use the second expression from (B.6) to verify that

Rn = n

This establishes that n is an eigen-vector with eigen-value 1.4. Since ñ ñ is a symmetric matrix, from (B.6) we obtain

R−R∗

= 2sin(θ)ñ

from which (B.9) follows.

Solution B.4 (pp. 406): Angular velocity from the angle/axis ratesDifferentiating n∗n = 1, we obtain n∗n = 0. Thus

ñññ = ñ[nn∗−n∗nI] = 0 (C.52)

Also, it is easy to verify thatñ ññ = − ñ (C.53)

Using these cross product identities, we have,

B ω=BRI

IRB =

[

I3 +(

1 − cos(θ))

ñ ñ− sin(θ)ñ]×

[

(

sin(θ) ñ ñ+ cos(θ) ñ)

θ+(

sin(θ)+ (1 − cos(θ)) ñ)

ñ

+(1 − cos(θ)) ñ ñ]

=

[

sin(θ)ñ ñ+ cos(θ)ñ− sin2(θ)ñ ñ ñ− sin(θ)cos(θ) ñ ñ

+ sin(θ)(1 − cos(θ)) ñ ñ ñ ñ+ cos(θ)(1 − cos(θ)) ñ ñ ñ]

θ

+ sin(θ) ñ+(1 − cos(θ)) ñ ñ− sin2(θ) ñ ñ− sin(θ)(1 − cos(θ)) ñ ñ ñ

+ sin(θ)(1 − cos(θ)) ñ ñ ñ+(1 − cos(θ))2ñ ñ ñ ñ+(1 − cos(θ)) ñ ñ

− sin(θ)(1 − cos(θ)) ñ ñ ñ+(1 − cos(θ))2ñ ñ ñ ñ

C.52,C.53= ñθ+ sin(θ)ñ+(1 − cos(θ)) ñ ñ− sin2

(θ)ñ ñ

−(1 − cos(θ))2ñ ñ+(1 − cos(θ)) ñ ñ

= ñθ+ sin(θ) ñ−(1 − cos(θ)) ñ ñ+(1 − cos(θ))ñ ñ

= ñθ+ sin(θ) ñ−(1 − cos(θ))˜ñn

HenceBω= nθ+ sin(θ)n−(1 − cos(θ)) ñn (C.54)

This establishes (B.10).

C Solutions 465

Solution B.5 (pp. 406): Angle/axis rates from the angular velocityWe have

[


[

12

[

ñ− cot(θ/2)ñ ñ ñ]

n∗

]

=12

[

−(1 − cosθ) ñ ñ+(1 − cosθ)cot(θ/2)ñ ñ

+ sin(θ)ñ− sin(θ)cot(θ/2)ñ ñ]

+nn∗(C.55)

However,

ñ ñ = nn∗− I, ñññ = − ñ

sin(θ)cot(θ/2) = 2cos2(θ/2), (1 − cosθ)cot(θ/2) = sin(θ/2)

Using these in (C.55) leads to

[


[

12

[

ñ− cot(θ/2)ñ ñ]

n∗

]

=12

[

−(1 − cosθ)ñ ñ− sinθ)ñ+ sin(θ)ñ−(1 + cos(θ/2))ñ ñ]

+nn∗

= −ññ+nn∗= I

This implies that expressions in (B.11) and (B.10) are indeed inverses of each other,establishing the result.

Solution B.6 (pp. 407): Quaternion expression for a rotation matrixUsing cos(θ) = 2cos2(θ/2) − 1 = 1 − 2sin2(θ/2), sin(θ) = 2sin(θ/2)cos(θ/2),and (B.12) in (B.6), we obtain

IRB(n,θ) = cos(θ)I3 +

[

1 − cos(θ)]

nn∗+ sin(θ)ñ

B.12= (2q2

0 − 1)I3 −qq∗ + 2q0 qB.12= (q2

0 −q∗q)I3 −qq∗ + 2q0 q

This establishes the first equality in (B.14). The remaining equalities follow fromusing, and manipulating, 3-vector cross-product identities.

466 C Solutions

Solution B.7 (pp. 407): Basic properties of unit quaternions

1. We have

R(q)R∗(q)

B.14= [I+ 2 q2

]2− 4q2

0 q2

= I+ 4 q2+ 4 q4

− 4q20 q

2

= I+ 4 q2(I+ q2

−q20I)

A.1= I+ 4 q2

(q∗qI+qq∗ −q∗qI) = I

2. It is easy to verify using (B.14) that R(q) = R(−q).3. Once again it is easy to check from (B.14) that R(q0,−q) = R∗(q0,q). Since

the inverse of a rotation matrix is its transpose, the result follows.4. We have

Trace{R(q)

} B.7= 1 + 2cos(θ) = 1 + 4cos2

(θ/2)− 2B.12= 4q2

0 − 1

establishing (B.16).5. Exercise B.3 established that n is an eigen-vector of R with eigen-value 1. Sinceq= sin(θ/2)n, q is also an eigen-vector of R with eigen-value 1.

6. From (B.9) we obtain

qB.12= sin(θ/2)(R−R

∗)/(2sin(θ)) = (R−R

∗)/(4cos(θ/2))

B.12= (R−R

∗)/(4q0)

This establishes (B.18).

Solution B.8 (pp. 409): Properties of E− and E+ matrices

1. The identities in (B.20) are established by evaluating and simplifying the variousproducts.

2. Similarly, the identities in (B.21) are established by evaluating and simplifyingthe various products.

3. These identities are easily established by examining (B.19).4. We have

E+(p)q =

[

p0q+pq0 + pq

p0q0 −p∗q

]

=

(

q0I− q q

−q∗ q0

)[

p

p0

]

= E−(q)p

establishing (B.23).

C Solutions 467

5. We have

E−(q)E+(p)

=

(

q0I3 − q q

−q∗ q0

)(

p0I3 + p p

−p∗ p0

)

=

(

(q0I3 − q)(p0I3 + p)−qp∗ (q0I3 − q)p+qp0

−q∗(p0I3 + p)−q0p∗ −q∗p+q0p0

)

=

(

q0p0I3 − qp0 +q0 p− q p−qp∗ (q0p− qp+qp0

−q∗p0 −q∗ p−q0p∗ −q∗p+q0p0

)

(C.56)

Similarly,

E+(p)E−(q)

=

(

p0I3 + p p

−p∗ p0

)(

q0I3 − q q

−q∗ q0

)

=

(

q0p0I3 − qp0 +q0 p− p q−pq∗ (q0p+ pq+qp0

−q∗p0 +p∗ q−q0p∗ −q∗p+q0p0

)

Comparing the matrix terms in this expression with those in (C.56) shows thatthey are the same. This establishes (B.24a).A similar explicit evaluation process establishes (B.24b).(B.24c) is simply the transpose of (B.24b), while (B.24d) is the transpose of(B.24a).

6. Since q is an eigen-vector of R(q,q0) from (B.17), (B.25) follows directly fromthe expression for T(q) in (B.20).

Solution B.9 (pp. 409): Composition of unit quaternions

1. The (B.27) follows from direct evaluation of the product in (B.26).The unit norm property of r = p⊗q follows from

‖r‖2= r∗r B.27

= q∗E∗+(p)E+(p)qB.20= q∗q = 1

2. Define r�= p⊗q = (r,r0). The (B.28) identity can be established by the brute

force use of (B.14) on both sides of the equation and a series of algebraic manip-ulations. We however, use a simpler approach, and show that r defined by (B.27)is an eigen-vector of T(p)T(q) with eigenvalue 1 as required by (B.25).have

468 C Solutions

T(p)T(q) =

(

R(p)R(q) 0

0 1

)

B.21= E+(p)E∗−(p)E+(q)E∗−(q)

Thus,

T(p)T(q)rB.26= E+(p)E∗−(p)E+(q)E∗−(q)E−(q)p

B.14= E+(p)E∗−(p)E+(q)p

B.26= E+(p)E∗−(p)E−(p)q

B.14= E+(p)q

B.26= r

This establishes that r is an eigen-vector corresponding to the eigen-value of 1,and hence it is the quaternion corresponding to the T(p)T(q) rotational trans-formation and verifies (B.28).

3. Now

p⊗ (q⊗r)B.26= E+(p)E−(r)q

B.24a= E−(r)E+(p)q

B.26= E−(r)(p⊗q)

B.26= (p⊗q)⊗r

4. We have

p−1 ⊗qB.26= E+(p−1

)qB.22= E∗+(p)q

establishing the first half of (B.30). For the latter half,

p⊗q−1 B.26= E−(q−1

)pB.22= E∗−(q)p

Solution B.10 (pp. 409): The quaternion identity elementIt is easy to verify that E+(eq) = E−(eq) = I4. Thus

q⊗eqB.26= E−(eq)q = q

The latter identity in (B.31) can be established by explicitly evaluating E+(q)q−1

and verifying that it is simply eq.

Solution B.11 (pp. 410): Unit quaternion products and inverses

1. Now

(p⊗q)⊗ (q−1 ⊗p−1)

B.29= p⊗ (q⊗q−1

)⊗p−1 B.31= p⊗p−1

= eq

This establishes that (q−1 ⊗p−1) is the inverse of p⊗q.

C Solutions 469

2. Now rB.26= E+(p)q. Thus

qB.20= E∗+(p)r

B.22= E+(p−1

)rB.26= p−1 ⊗r

A similar derivation leads to p = r⊗q−1.

Solution B.12 (pp. 410): Transforming vectors with unit quaternionsWe have[

Ix

0

]

=

(

IRB(q) 0

0 1

)[

Bx

0

]

B.21= E+(q)E∗−(q)

[

Bx

0

]

B.22= E+(q)E−(q−1

)

[

Bx

0

]

B.26= q⊗

[

Bx

0

]

⊗q−1

Solution B.13 (pp. 410): Quaternion rates from the angular velocityWith q0 = cos(θ/2),

q0 = −12

sin(θ/2)θB.11= −

12

sin(θ/2)n∗Bω= −12q∗Bω

This establishes the lower half of the first equality in (B.34).Also, with q= sin(θ/2)n,

q=12

cos(θ/2)θn+ sin(θ/2)n

B.11=

12q0nn∗Bω+

sin(θ/2)

2

[

ñ− cot(θ/2)ñ ñ]

Bω

=12

[q0nn∗+ q− cos(θ/2)ññ]Bω

=12

[q0nn∗+ q−q0 ñ ñ]Bω

=12

[q0(nn∗− ñ ñ)+ q]Bω=

12

[q0I+ q]Bω

This establishes the upper half of the first equality in (B.34).The second equality in (B.34) is a rearrangement of the first one.The third equality follows by using the definition of E+(q) from (B.19).The last equality follows from (B.26).

470 C Solutions

Solution B.14 (pp. 410): Constant unit quaternion norms

1. The rate of change of the norm of q(t) is given by

d‖q(t)‖2

dt=

12

dq∗qdt

= q∗q B.34=

12q∗E+(q)

[

Bω

0

]

B.22,B.26=

12e∗

q

[

Bω

0

]

= 0

2. From the above we have that

0 = q∗q = q∗ q+q0q0

from which (B.35) follows.

Solution B.15 (pp. 411): Unit quaternion rate to angular velocity

1. (B.36) follows by using (B.32) in (B.34). From this it follows that

BωB.19= 2 [q0I3 − q] q−qq0

B.35= 2 [q0I3 − q] q+q(q∗ q/q0)

from which (B.37) follows.2. We have

[

Iω

0

]

B.33= q⊗

[

Bω

0

]

⊗q−1 B.36= 2q⊗q−1 B.30

= 2E∗−q

establishing (B.39). From this it follows that

IωB.19= 2 [q0I3 + q] q−qq0

B.35= 2 [q0I3 + q] q+q(q∗ q/q0)

establishing (B.41).

Solution B.16 (pp. 411): Quaternion double time derivativesDifferentiating (B.34) we have

q =12q⊗

[

Bω

0

]

+12q⊗

[

Bω

0

]

B.34=

12q⊗

[

Bω

0

]

+12

{12q⊗

[

Bω

0

]}⊗[

Bω

0

]

B.27=

12q⊗

[

Bω

0

]

+14q⊗ (‖Bω‖2eq)

establishing (B.42).(B.43) follows by composing both sides of (B.42) with q−1 and rearranging terms.

C Solutions 471

Solution B.17 (pp. 412): Gibbs vector attitude representation

1. From Exercise (B.2) on page 405 it follows that

s3= −σ2 s, s5

= σ4 s, · · · s2k+1= (−1)

kσ2k s

Thus,s4

= −σ2 s2, s6= σ4 s2, · · · s2k+2

= (−1)kσ2k s2

Using these expressions for the powers of s, we have

[I− s ]−1= I+ s+ s2

+ · · · = I+

∞∑k=0

(−1)kσ2k

( s+ s2)

= I+( s+ s2)

∞∑k=0

(−1)kσ2k

= I+( s2+ s)/(1 +σ2

)

2.

R(s)B.44= (I+ s)+ s(I+ s)2/(1 +σ2

) = I+( s3+ 2 s2

+(2 +σ2) s)/(1 +σ2

)

B.5= I+ 2( s2

+ s)/(1 +σ2)

To establish that R(s) is a rotation matrix it suffices to verify that R(s)R∗(s) = I

and that detR(s) = 1. Since

I− s2= [I− s ][I+ s ] = [I+ s ][I− s ]

it follows that

R(s)B.45= [I− s ]−1

[I+ s ] = [I+ s ][I− s ]−1

and thatR

∗(s)

B.45= [I+ s ]−1

[I− s ]

Thus,R(s)R∗

(s) = [I− s ]−1[I+ s ][I+ s ]−1

[I− s ] = I

establishing the orthogonality of R(s). Furthermore,

d�= det [I− s ] = det [I− s ]∗ = det [I+ s ]

Hence, det [I− s ]−1 = 1/d. Putting these together we have

detR(s) = det [I− s ]−1 det [I+ s ] = (1/d)d= 1

This establishes that R(s) is a rotation matrix since it is orthogonal and hasdeterminant 1.

472 C Solutions

3. From (B.45) it follows that R−R s = I+ s from which the first half of (B.46)follows. Now let us assume that for some constants a,b and c to be determined,we have

−[I −R][I +R]−1= aR

2+bR+ cI

Then it follows that

−I +R = [I +R][aR2+bR+ cI]

= aR3+(a+b)R2

+(b+ c)R+ cI

B.8= ((1 +γ)a+b)R2

+(−aγ+b+ c)R+(a+c)I

For this to hold, the coefficients must satisfy the following linear equations:

(1 +γ)a+b= 0, −aγ+b+ c= 1, a+ c= −1

The solution to these equations is

a= −1/(1 +γ), b = 1, c= −γ/(1 +γ)

which establishes the result.4. For s to the axis of rotation, we need to show that it an eigen-vector of R(s) with

eigen-value 1. We have

R(s)sB.45=

[

I + 2( s2+ s)/(1 +σ2

)]

= s

This establishes the result.5. With s defined by (B.47), we have σ= tan(θ/2). Thus,

R(s)B.45= I+ 2(σ2

ñ2+σñ)/(1 +σ2

)

= I+ 2cos2(θ/2)(tan2

(θ/2)ñ2+ tan(θ/2)ñ)

= I+(1 − cos(θ)) ñ2+ sin(θ)ñ

The last expression agrees with the expression for a rotation matrix in (B.6) estab-lishing the result.

Solution B.18 (pp. 413): Rotation of vectors

1. Since R(s)a= b,

bB.45= [I− s ]−1

[I+ s ]a =⇒ [I− s ]b= [I+ s ]a

from which (B.48) follows.2. Since any vector s satisfying (B.48) will generate a rotation matrix R(s) with the

desired properties, we look for the general solution to the linear matrix equation

C Solutions 473

in (B.48). We now verify that sp�= λ ˜(a−b)(a+b) is a particular solution to

the linear equations. Since a and b have the same norm, the (a+b) and (a−b)

vectors are mutually orthogonal because

(a+b)∗(a−b) = a∗a−a∗b+b∗a−b∗b = 0

Now

˜(a+b)sp = −λ ˜(a+b) ˜(a+b)(a−b)

A.1= −λ

[

(a+b)(a+b)∗−‖a+b‖2I]

(a−b) = (a−b)

Thus, sp is indeed a particular solution for (B.48). To determine all the remainingsolutions observe that the class of homogeneous solutions for (B.48) is simplys = α(a+b) for an arbitrary scalar α. Combining the particular and homoge-neous solution leads to the general solution in (B.49).

References

[1] Aghili, F.: Simplified Lagrangian mechanical systems with constraints using square-rootfactorization. In: Multibody Dynamics 2007, ECCOMAS Thematic Conference (2007)

[2] Aghili, F.: A gauge-invariant formulation for constrained robotic systems using square-rootfactorization and unitary transformation. In: IEEE/RSJ International Conference on Intelli-gent Robots and Systems, pp. 2814–2821 (2008)

[3] Aghili, F.: Dynamics and control of constrained mechanical systems in terms of re-duced quasi-velocities. In: IEEE International Conference on Robotics and Automation,pp. 1225–1232 (2008)

[4] Alexander, H., Cannon, R.: Experiments on the control of a satellite Manipulator. In: Pro-ceedings of American Control Conference (1987)

[5] Anderson, B.D.O., Moore J.B.: Optimal Filtering. Prentice-Hall, Englewood Cliffs, NJ(1979)

[6] Anderson, K.: An order-n formulation for the motion simulation of general multi-rigid-bodytree systems. Comput. Struct. 46(3), 547–559 (1993)

[7] Arai, H., Tachi, S.: Position control of manipulator with passive joints using dynamic cou-pling. IEEE Trans. Robot. Autom. 7, 528–534 (1991)

[8] Arimoto, S., Miyazaki, F.: Stability and Robustness of PID Feedback Control for RobotManipulators of Sensory Capability. MIT Press, Cambridge, MA (1983)

[9] Armstrong, B., Khatib, O., Burdick, J.: The explicit model and inertial parameters of thePUMA 560 Arm. In: IEEE International Conference on Robotics and Automation, SanFrancisco, CA (1986)

[10] Armstrong, W.: Recursive solution to the equations of motion of an N-link Manipulator. In:5th World Congress on Theory of Machines and Mechanisms, Montreal, Canada, vol. 2,pp. 1343–1346 (1979)

[11] Arnold, V.I.: Mathematical Methods of Classical Mechanics. Springer, Heidelberg (1984)[12] Asada, H., Slotine, J.J.E.: Robot Analysis and Control. Wiley, New York (1986)[13] Ascher, U.M., Pai, D., Cloutier, B.: Forward dynamics, elimination methods, and formula-

tion stiffness in robotic simulation. Int. J. Robot. Res. 16(6), 749–758 (1997)[14] Ascher, U.M., Petzold, L.R.: Computer methods for ordinary differential equations and

differential-algebraic equations. SIAM (1998)[15] Austin, M., Krishnaprasad, P., Wang, L.S.: Almost Poisson integration of rigid body sys-

tems. Technical Report TR 91-45r1, Systems Research Center, University of Maryland,MD (1991)

[16] Bae, D., Haug, E.: A recursive formulation for constrained mechanical system dynamics:Part I. Open loop systems. Mech. Struct. Mach. 15(3), 359–382 (1987)

[17] Bae, D., Haug, E.: A recursive formulation for constrained mechanical system dynamics:Part II. Closed loop systems. Mech. Struct. Mach. 15(4), 481–506 (1987–1988)

475

476 References

[18] Baillieul, J.: The nonlinear control theory of super-articulated mechanisms. In: Proceedingsof American Control Conference, San Diego, CA, pp. 2448–2451 (1990)

[19] Balafoutis, C., Misra, P., Patel, R.: Recursive evaluation of dynamic robot models. IEEEJ. Robot. Autom. 2(3), 146–155 (1986)

[20] Balakrishnan, A.: Applied Functional Analysis. Springer, New York (1976)[21] Banerjee, A.K.: Contributions of Multibody Dynamics to Space Flight: A Brief Review.

J. Guid. Control Dynam. 26(3), 385–394 (2003)[22] Banerjee, A.K., Dickens, J.: Dynamics of an arbitrary flexible body in large rotation and

translation. J. Guid. Control Dynam. 13(2), 221–227 (1990)[23] Baraff, D.: Linear-time dynamics using Lagrange multipliers. In: Proceedings of ACM SIG-

GRAPH ’96, New Orleans, pp. 137–146 (1996)[24] Baumgarte, J.: Stabilization of constraints and integrals of motion in dynamical systems.

Comput. Method. Appl. Mech. Eng. 1, 1–16 (1972)[25] Bedrossian, N.: Linearizing coordinate transformations and euclidean systems. In: Work-

shop on Nonlinear Control of Articulated Flexible Structures, Santa Barbara, CA (1991)[26] Bodley, C.S., Devers, A.D., Park, A.C., Frisch, H.P.: A digital computer program for the

dynamic interaction simulation of controls and structure (DISCOS). NASA Technical Paper1219, NASA (1978)

[27] Bowling, A., Khatib, O.: Design of macro/mini manipulators for optimal dynamic perfor-mance. In: IEEE International Conference on Robotics and Automation, pp. 449–454 (1997)

[28] Brandl, H., Johanni, R., Otter, M.: A very efficient algorithm for the simulation of robotsand similar multibody systems without inversion of the mass matrix. In: IFAC/IFIP/IMACSSymposium (1st), Vienna, Austria, pp. 95–100 (1986)

[29] Brandl, H., Johanni, R., Otter, M.: An algorithm for the simulation of multibody systemswith kinematic loops. In: World Congress on the Theory of Machines and Mechanisms(7th), Seville, Spain (1987)

[30] Breckenridge, W.: Quaternions – proposed standard conventions. IOM 343-79-1199, JetPropulsion Laboratory (1979)

[31] Brogliato, B.: Nonsmooth Mechanics. Springer, London (1999)[32] Burdick, J.: An algorithm for generation of efficient manipulator dynamic equations. In:

IEEE International Conference on Robotics and Automation, San Francisco, CA (1986)[33] Chang, C., Nikravesh, P.: Optimal design of mechanical systems with constraint violation

stabilization method. J. Mech. Transm. Autom. Des. 107, 493–498 (1985)[34] Chang, K.S., Khatib, O.: Efficient recursive algorithm for the operational space inertia ma-

trix of branching mechanisms. Adv. Robot. 4(8), 703–715 (2001)[35] Changizi, K., Shabana, A.: A recursive formulation for the dynamic analysis of open loop

deformable multibody systems. ASME J. Appl. Mech. 55(3), 687–693 (1988)[36] Chen, J.: The effects of gear reduction on robot dynamics. In: Rodriguez, G., Seraji, H.

(eds.) NASA Conference on Space Telerobotics, Pasadena, CA (Submitted to ASME J.Dyn. Meas. Contr.) (1989)

[37] Chen, J., Ou, J.: Development of efficient computer program for dynamic simulation oftelerobotic manipulation. In: Proceedings of the 3rd Annual Conference on Aerospace Com-putational Control, Oxnard, CA, vol. 1, pp. 248–262, (JPL Publication 89–45, 1989) (1989)

[38] Cherowitzo, W.: Graph and Digraph Glossary. http://www-math.cudenver.edu/∼wcherowi/courses/m4408/glossary.htm/ (2009)

[39] Chirikjian, G., Burdick, J.: Kinematics of hyper-redundant locomotion with applications tograsping. In: IEEE International Conference on Robotics and Automation, Sacramento, CA,pp. 720–725 (1991)

[40] Cottle, R., Pang, J., Stone, R.: The Linear Complementarity Problem. Academic, New York(1992)

[41] Craig, J.J.: Introduction to Robotics. Addison-Wesley, Reading, MA (1986)[42] Critchley, R., Anderson, K.: A generalized recursive coordinate reduction method for multi-

body dynamic systems. Int. J. Multiscale Comput. Eng. 12(2), 181–200 (2003)[43] Diebel, J.: Representing attitude: Euler angles, unit quaternions, and rotation vectors.

http://citeseerx.ist.psu.edu/viewdoc/summary?doi=?doi=10.1.1.110.5134 (2006)

http://www-math.cudenver.edu/~wcherowi/courses/m4408/glossary.htm/

http://www-math.cudenver.edu/~wcherowi/courses/m4408/glossary.htm/

http://citeseerx.ist.psu.edu/viewdoc/summary?doi=?doi=10.1.1.110.5134

References 477

[44] Dubowsky, S., Vafa, Z.: A virtual manipulator model for space robotic systems. In:Rodriguez, G. (ed.) Proceedings of the Workshop on Space Telerobotics, Pasadena, CA(1987)

[45] Duindam, V., Stramigioli, S.: Singularity-free dynamic equations of open-chain mecha-nisms with general holonomic and nonholonomic joints. IEEE Trans. Robot. Autom. 24(3),517–526 (2008)

[46] Eisenhart, L.: Riemannian Geometry. Princeton University Press, Princeton (1960)[47] Featherstone, R.: The calculation of robot dynamics using articulated-body inertias. Int. J.

Robot. Res. 2(1), 13–30 (1983)[48] Featherstone, R.: Robot Dynamics Algorithms. Kluwer, Boston, MA (1987)[49] Featherstone, R.: A divide-and-conquer articulated-body algorithm for parallel

O(log(n)) calculation of rigid-body dynamics. Part 1: Basic algorithm. Int. J. Robot.Res. 18(9), 867–875 (1999)

[50] Featherstone, R.: A divide-and-conquer articulated-body algorithm for parallelO(log(n)) calculation of rigid-body dynamics. Part 2: Trees, loops, and accuracy.Int. J. Robot. Res. 18(9), 876–892 (1999)

[51] Featherstone, R.: Rigid Body Dynamics Algorithms. Springer, New York (2008)[52] Featherstone, R.: Exploiting sparsity in operational-space dynamics. Int. J. Robot. Res. 29,

1353–1368 (2010)[53] Featherstone, R., Orin, D.: Robot dynamics: equations and algorithms. In: IEEE Interna-

tional Conference on Robotics and Automation, San Francisco, CA, pp. 826–834 (2000)[54] Fijany, A.: Parallel algorithms and architectures in robotics. PhD thesis, University of Orsay

(Paris XI) (1988)[55] Fijany, A., Bejczy, A.: Parallel algorithms and architectures for manipulator inverse dynam-

ics. In: OSU Conference, Columbus, OH (1989)[56] Fijany, A., Scheid, R.: Efficient conjugate gradient algorithms for computation of the manip-

ulator forward dynamics. In: Rodriguez, G., Seraji, H. (eds.) NASA Conference on SpaceTelerobotics, Pasadena, CA (1989)

[57] Fixman, M.: Simulation of polymer dynamics. I. General theory. J. Chem. Phys. 69(4),1527–1537 (1978)

[58] Fraser, D., Potter, J.: The optimum linear smoother as a combination of two optimum linearfilters. IEEE Trans. Autom. Contr. 387–390 (1969)

[59] Gelb, A.: Applied Optimal Estimation. MIT Press, Cambridge, MA (1974)[60] Goldstein, H.: Classical Mechanics. Addison-Wesley, Reading, MA (1980)[61] Graham, A.: Kronecker Products and Matrix Calculus: With Applications. Horwood

Halsted Press, New York (1981)[62] Hardt, M.W.: Multibody dynamical algorithms, numerical optimal control, with detailed

studies in the control of jet engine compressors and biped walking. PhD thesis, Universityof California, San Diego (1999)

[63] Herman, P., Kozlowski, K.: A survey of equations of motion in terms of inertial quasi-velocities for serial manipulators. Arch. Appl. Mech. 76, 579–614 (2006)

[64] Ho, J.: Direct path method for flexible multibody spacecraft dynamics. J. Spacecraft Rockets(AIAA) 14(2), 102–110 (1977)

[65] Ho, J., Herber, D.: Development of dynamics and control simulation of large flexible spacesystems. J. Guid. Control Dyn. 8(3), 374–383 (1985)

[66] Hollerbach, J.: A recursive Lagrangian formulation of manipulator dynamics and a compar-ative study of dynamics formulation complexity. IEEE Trans. Syst. Man Cybern. 10(11),730–736 (1980)

[67] Hsu, Y., Anderson, K.: Low operational order analytic sensitivity analysis for tree-type muti-body dynamics systems. J. Guid. Control Dyn. 24(6), 1133–1143 (2001)

[68] Hu, W., Marhefka, D.W., Orin, D.E.: Hybrid kinematic and dynamic simulation of runningmachines. IEEE Trans. Robot. Autom. 21(3), 490–497 (2010)

[69] Hughes, P.: Spacecraft Attitude Dynamics. Wiley, New York (1986)[70] Huston, R.: Multibody Dynamics. Butterworth-Heinmann, Stoneham, MA (1990)

478 References

[71] Jain, A.: Unified formulation of dynamics for serial rigid multibody systems. J. Guid.Control Dyn. 14(3), 531–542 (1991)

[72] Jain, A.: Compensating mass matrix potential for constrained molecular dynamics.J. Comput. Phys. 136, 289–297 (1997)

[73] Jain, A.: Correction to ‘spatial operator algebra for multibody system dynamics’. http://dartslab.jpl.nasa.gov/References/pdf/gen-mbody-correction.pdf (2009)

[74] Jain, A.: Recursive algorithms using local constraint embedding for multibody system dy-namics. In: ASME 2009 International Design Engineering Technical Conferences & Com-puters and Information in Engineering Conference, San Diego, CA (2009)

[75] Jain, A.: Graph theoretic structure of multibody system spatial operators. In: The 1st JointInternational Conference on Multibody System Dynamics, Lappeenranta, Finland (2010)

[76] Jain, A.: Graph theory roots of spatial operators for kinematics and dynamics. In: IEEEInternational Conference on Robotics and Automation, Anchorage, Alaska (2010)

[77] Jain, A., Rodriguez, G.: Recursive dynamics for geared robotic manipulators. In: IEEE Con-ference on Decision and Control, Honolulu, Hawaii (1990)

[78] Jain, A., Rodriguez, G.: Kinematics and dynamics of under-actuated manipulators. In: IEEEInternational Conference on Robotics and Automation, Sacramento, CA, pp. 1754–1759(1991)

[79] Jain, A., Rodriguez, G.: Recursive flexible multibody system dynamics using spatial opera-tors. J. Guid. Control Dyn. 15(6), 1453–1466 (1992)

[80] Jain, A., Rodriguez, G.: An analysis of the kinematics and dynamics of underactuated ma-nipulators. IEEE Trans. Robot. Autom. 9(4), 411–422 (1993)

[81] Jain, A., Rodriguez, G.: Linearization of manipulator dynamics using spatial operators.IEEE Trans. Syst. Man Cybern. 23(1), 239–248 (1993)

[82] Jain, A., Rodriguez, G.: Recursive dynamics algorithm for multibody systems with pre-scribed motion. J. Guid. Control Dyn. 16(5), 830–837 (1993)

[83] Jain, A., Rodriguez, G.: Base-invariant symmetric dynamics of free-flying space manipula-tors. IEEE Trans. Robot. Autom. 11(4), 585–597 (1995)

[84] Jain, A., Rodriguez, G.: Diagonalized Lagrangian robot dynamics. IEEE Trans. Robot. Au-tom. 11(4), 571–584 (1995)

[85] Jain, A., Rodriguez, G.: Multibody mass matrix sensitivity analysis using spatial operators.Int. J. Multiscale Comput. Eng. 1(2–3) (2003)

[86] Jain, A., Kreutz, K., Rodriguez, G.: Multi-Arm grasp and manipulation of objects with in-ternal degrees of freedom. In: IEEE Conference on Decision and Control, Honolulu, Hawaii(1990)

[87] Jain, A., Kreutz, K., Rodriguez, G.: Recursive dynamics of multiarm robotic systems inloose grasp of articulated task objects. In: 3rd International Symposium on Robotics andManufacturing, Vancouver, Canada (1990)

[88] Jain, A., Vaidehi, N., Rodriguez, G.: A fast recursive algorithm for molecular dynamicssimulations. J. Comput. Phys. 106(2), 258–268 (1993)

[89] de Jalon, J.G., Alvarez, E., de Ribera, F.A., Rodriguez, I., Funes, F.J.: A fast and simplesemi-recursive dynamic formulation for multi-rigid-body systems. Adv. Comput. MultibodySyst., 1–24 (2005)

[90] Junkins, J.L., Schaub, H.: An instantaneous eigenstructure quasivelocity formulation fornonlinear multibody dynamics. J. Astronaut. Sci. 45(3), 279–295 (1997)

[91] Kailath, T.: An innovations approach to least-squares estimation. IEEE Trans. Inform. The-ory. IT–14 (1968)

[92] Kailath, T.: Fredholm resolvents, Wiener-Hopf equations, and Riccati differential equations.IEEE Trans. Inform. Theory 15, 665–672 (1969)

[93] Kailath, T.: A view of three decades of linear filtering theory. IEEE Trans. Inform. Theory20, 147–181 (1974)

[94] Kamman, J., Huston, R.: Dynamics of constrained multibody systems. ASME J. Appl.Mech. 51, 889–903 (1984)

[95] Kane, T., Levinson, D.: The use of Kane’s dynamical equations in robotics. Int. J. Robot.Res. 2(3), 3–21 (1983)

http://dartslab.jpl.nasa.gov/References/pdf/gen-mbody-correction.pdf

http://dartslab.jpl.nasa.gov/References/pdf/gen-mbody-correction.pdf

References 479

[96] Kane, T., Levinson, D.: Dynamics: Theory and Applications. McGraw-Hill, New York(1985)

[97] Kane, T., Likins, P., Levinson, D.: Spacecraft Dynamics. McGraw-Hill, New York (1983)[98] Kane, T., Ryan, R., Banerjee, A.: Dynamics of a cantilevered beam attached to a moving

base. J. Guid. Control Dyn. 10(2), 139–151 (1987)[99] Keat, J.: Multibody system order n dynamics formulation based in velocity transform

method. J. Guid. Control Dyn. 13(2), 201–212 (1990)[100] Khatib, O.: Dynamic control of manipulators in operational space. In: 6th CISM-IFToMM

Congress on Theory of Machines and Mechanisms, New Delhi, India (1983)[101] Khatib, O.: The operational space formulation in the analysis, design, and control of manip-

ulators. In: 3rd International Symposium Robotics Research, Paris (1985)[102] Khatib, O.: A unified approach for motion and force control of robot manipulators: The

operational space formulation. IEEE J. Robot. Autom. 3(1), 43–53 (1987)[103] Khatib, O.: Object manipulation in a multi-effector system. In: 4th International Symposium

on Robotics Research, Santa Cruz, CA, pp. 137–144 (1988)[104] Kim, S., Haug, E., A recursive formulation for flexible multibody dynamics, Part I: open-

loop systems. Comput. Meth. Appl. Mech. Eng. 71(3), 293–314 (1988)[105] Koditschek, D.: Robot kinematics and coordinate transformations. In: IEEE Conference on

Decision and Control, Ft. Lauderdale, FL, pp. 1–4 (1985)[106] Kozlowski, K., Herman, P.: Control of robot manipulators in terms of quasi-velocities.

J. Intell. Robot. Syst. 53(3), 205–221 (2008)[107] Kreutz, K.: On manipulator control by exact linearization. IEEE Trans. Autom. Contr. 34,

763–767 (1989)[108] Kreutz, K., Lokshin, A.: Load balancing and closed-chain multiple arm control. In:

American Control Conference, Atlanta, GA (1988)[109] Kreutz, K., Wen, J.: Attitude control of an object commonly held by multiple robot arms: a

Lyapunov approach. In: American Control Conference, Atlanta, GA (1988)[110] Kreutz-Delgado, K., Jain, A., Rodriguez, G.: Recursive formulation of operational space

control. Int. J. Robot. Res. 11(4), 320–328 (1992)[111] Kwatny, H.G., Blankenship, G.: Nonlinear Control and Analytical Mechanics: A Computa-

tional Approach. Birkhauser, Boston (2000)[112] Lanczos, C.: The Variational Principles of Mechanics. University of Toronto Press, Toronto

(1949)[113] Lathrop, R.: Constrained (closed-loop) robot simulation by local constraint propagation.

In: IEEE International Conference on Robotics and Automation, San Francisco, CA,pp. 689–694 (1986)

[114] Lee, C.S.G., Chang, P.R.: Efficient parallel algorithms for robot inverse dynamics computa-tions. IEEE Trans. Syst. Man Cybern. 16, 532–542 (1986)

[115] Lee, C.S.G., Chang, P.R.: Efficient algorithms for robot forward dynamics computations.In: IEEE International Conference on Robotics and Automation, Raleigh, NC, pp. 654–699(1987)

[116] Li, C.J.: An efficient method for linearization of dynamic models of robot manipulators.IEEE Trans. Robot. Autom. 5(4), 397–408 (1989)

[117] Li, C.J., Hemami, A., Sankar, T.: A new computational method for linearized dynamic mod-els for robot manipulators. Int. J. Robot. Res. 9(1), 134–144 (1990)

[118] Likins, P.: Modal method for analysis of free rotations of spacecraft. AIAA J. 5, 1304–1308(1967)

[119] Lovelock, D., Rund, H.: Tensors, Differential Forms, and Variational Principles. Dover,New York (1989)

[120] Luh, J., Walker, M., Paul, R.: On-line computational scheme for mechanical manipulators.ASME J. Dyn. Syst. Meas. Contr. 102(2), 69–76 (1980)

[121] Mani, N., Haug, E., Atkinson, K.: Application of singular value decomposition for analysisof mechanical system dynamics. J. Mech. Trans. Automat. Des. 107, 82–87 (1985)

480 References

[122] Mariti, L., Pennestri, E., Valentini, P.P., Belfiore, N.P.: Review and comparison of solutionstrategies for multibody dynamics equations. In: The 1st Joint International Conference onMultibody System Dynamics, Lappeenranta, Finland (2010)

[123] McPhee, J.J., Ishac, M.G., Andrews, G.C.: Wittenburg’s formulation of multibody dynamicsequations from a graph-theoretic perspective. Mech. Mach. Theory 31(2), 201–213 (1996)

[124] Meirovitch, L., Methods of Analytical Dynamics. McGraw-Hill, New York (1970)[125] Mukherjee, R., Nakamura, Y.: Formulation and efficient computation of inverse dynamics

of space robots. In: IEEE Trans. Robot. Autom. 8, 400–406 (1992)[126] Murphy, S.H.: Modeling and simulation of multiple manipulators on a mobile platform.

PhD thesis, R.P.I., Troy, NY (1990)[127] Murphy, S.H., Wen, J., Saridis, G.: Recursive calculation of geared robot manipulator dy-

namics. In: IEEE International Conference on Robotics and Automation, Cincinnati, OH,pp. 839–844 (1990)

[128] Murray, R., Li, Z., Sastry, S.: A Mathematical Introduction to Robotic Manipulation. CRCPress, Boca Raton, FL (1994)

[129] Nakamura, Y., Mukherjee, R.: Nonholonomic path planning for space robots. In: IEEE In-ternational Conference on Robotics and Automation, Scottsdale, AZ (1989)

[130] Nakamura, Y., Mukherjee, R.: Nonholonomic path planning for space robot via bi-directional approach. In: IEEE International Conference on Robotics and Automation,Cincinnati, OH (1990)

[131] Nguyen, B., Trinkle, J.C.: dvc3D: a three dimensional physical simulation tool for rigidbodies with contacts and Coulomb friction. In: The 1st Joint International Conference onMultibody System Dynamics, Lappeenranta, Finland (2010)

[132] Orlandea, N., Chace, M., Calahan, D.: A sparsity-oriented approach to the dynamic analysisand design of mechanical systems – Part 1. ASME J. Eng. Ind. 99(2), 773–779 (1977)

[133] Padilla, C.E., von Flotow, A.H.: Nonlinear strain–displacement relations and flexible multi-body dynamics. In: Proceedings of the 3rd Annual Conference on Aerospace ComputationalControl, vol. 1, Oxnard, CA, pp. 230–245, (JPL Publication 89–45, Jet Propulsion Labora-tory, Pasadena, CA, 1989) (1989)

[134] Papadopoulos, E., Dubowsky, S.: On the nature of control algorthms for space manipulators.In: IEEE International Conference on Robotics and Automation, Cincinnati, OH (1990)

[135] Paul, R.P.: Robot Manipulators: Mathematics, Programming, and Control. MIT Press,Cambridge, MA (1982)

[136] Pfeiffer, F.: Mechanical System Dynamics. Springer, Berlin (2005)[137] Ramoni, M.F.: Bayesian Networks Representation and Reasoning. http://ocw.mit.edu/NR/

rdonlyres/Health-Sciences-and-Technology/HST-951JMedical-Decision-SupportSpring2003/695229E8-B5C8-4F33-A5B9-C4079AFAF967/0/lecture5.pdf (2009)

[138] Roberson, R., Schwertassek, R.: Dynamics of Multibody Systems. Springer, Berlin (1988)[139] Rodriguez, G.: Recursive forward dynamics for two robot arms in a closed chain based on

Kalman filtering and Bryson-Frazier smoothing. In: Jamshidi, M. (ed.) Robotics, North-Holland, Amsterdam (1986)

[140] Rodriguez, G.: Kalman filtering, smoothing and recursive robot arm forward and inversedynamics. IEEE J. Robot. Autom. 3(6), 624–639 (1987)

[141] Rodriguez, G.: Kalman filtering, smoothing, and topological tree dynamics. In: VPI/SUSymposium on Control of Large Structures, Blacksburg, VA (1987)

[142] Rodriguez, G.: Recursive multirigid body topological tree dynamics via Kalman filteringand Bryson-Frazier smoothing. In: VPI/SU Symposium on Control of Large Structures,Blacksburg, VA (1987)

[143] Rodriguez, G.: Spatially random models, estimation theory, and robot arm dynamics. In:Rodriguez G. (ed.) Proceedings of the Workshop on Space Telerobotics, Jet PropulsionLaboratory, Pasadena, CA (JPL Publication 87–13, 1987) (1987)

[144] Rodriguez, G.: Recursive forward dynamics for multiple robot arms moving a common taskobject. IEEE Trans. Robot. Autom. 5(4), 510–521 (1989)

[145] Rodriguez, G.: Random field estimation approach to robot dynamics. IEEE Trans. Syst.Man Cybern. 20(5), 1081–1093 (1990)

http://ocw.mit.edu/NR/rdonlyres/Health-Sciences-and-Technology/

http://ocw.mit.edu/NR/rdonlyres/Health-Sciences-and-Technology/

HST-951JMedical-Decision-SupportSpring

2003/695229E8-B5C8-4F33-A5B9-C4079AFAF967/0/lecture5.pdf

References 481

[146] Rodriguez, G.: Spatial operator approach to flexible multibody manipulator inverse andforward dynamics. In: IEEE International Conference on Robotics and Automation, Cincin-nati, OH (1990)

[147] Rodriguez, G.: Spatial operator approach to flexible multibody system dynamics and con-trol. In: 4th Annual Conference on Computational Control, Williamsburg, VA (1990)

[148] Rodriguez, G., Jain, A.: 2-Way recursive decomposition of the free-flying robot mass ma-trix. J. Appl. Math. Comput. Sci. 7(2), 101–119 (1997)

[149] Rodriguez, G., Kreutz-Delgado, K.: Spatial operator factorization and inversion of the ma-nipulator mass matrix. IEEE Trans. Robot. Autom. 8(1), 65–76 (1992)

[150] Rodriguez, G., Scheid, R.: Recursive inverse kinematics for robot arms via Kalman filteringand Bryson-Frazier smoothing. In: AIAA Guidance, Navigation, and Control Conference,Monterey, CA (1987)

[151] Rodriguez, G., Kreutz-Delgado, K., Jain, A.: A spatial operator algebra for manipulatormodeling and control. Int. J. Robot. Res. 10(4), 371–381 (1991)

[152] Rodriguez, G., Jain, A., Kreutz-Delgado, K.: Spatial operator algebra for multibody systemdynamics. J. Astronaut. Sci. 40(1), 27–50 (1992)

[153] Rosenberg, R.: Analytical Dynamics of Discrete Systems. Plenum, New York (1977)[154] Rosenthal, D.: Order N formulation for equations of motion of multibody systems. In:

SDIO/NASA Workshop on Multibody Simulations, Arcadia, CA (1987)[155] Rosenthal, D.: Triangularization of equations of motion for robotic systems. J. Guid. Control

Dyn. 11(3), 278–281 (1988)[156] Rosenthal, D.: An order n formulation for robotic systems. J. Astronaut. Sci. 38(4), 511–530

(1990)[157] Saha, S.K.: A decomposition of the manipulator inertia matrix. IEEE Trans. Robot. Autom.

13(2), 301–304 (1997)[158] Schwerin, RV.: Multibody System Simulation: Numerical Methods, Algorithms, and

Software. Springer, Berlin (1999)[159] Schwertassek, R.: Recursive generation of multibody system equations. In: SDIO/NASA

Workshop on Multibody Simulations, Arcadia, CA (1987)[160] Schwertassek, R.: Multibody kinematical equations in terms of relative variables. Z. Angew.

Math. Mech. 68(6), 207–211 (1988)[161] Schwertassek, R., Senger, K.: Representation of joints in multibody systems. Z. Angew.

Math. Mech. 68(2), 111–119 (1988)[162] Selig, J.: Geometrical Fundamentals of Robotics. Springer, New York (2004)[163] Shabana, A.A.: Dynamics of Multibody Systems. Cambridge University Press, Cambridge

(1998)[164] Shi, P., McPhee, J.J.: Dynamics of flexible multibody systems using virtual work and linear

graph theory. Multibody Syst. Dyn. 4, 355–381 (2000)[165] Shuster, M.: A survey of attitude representations. J. Astronaut. Sci. 41(4), 439–517 (1993)[166] Siciliano, B., Khatib, O.: Springer Handbook of Robotics. Springer, Heidelberg (2008)[167] Silver, W.: On the equivalence of Lagrangian and Newton–Euler dynamics for manipulators.

Int. J. Robot. Res. 1(2), 118–128 (1982)[168] Sinclair, A.J., Hurtado, J.E., Junkins, J.L.: Linear feedback control using quasi velocities. J.

Guid. Control Dyn. 29(6), 1309–1314 (2006)[169] Singh, R., VanderVoort, R., Likins, P.: Dynamics of flexible bodies in tree topology – a

computer-oriented approach. J. Guid. Control Dyn. 8(5), 584–590 (1985)[170] Spong, M.: Modeling and control of elastic joint robots. ASME J. Dyn. Syst. Meas.

Contr. 109, 310–319 (1989)[171] Spong, M.: Remarks on robot dynamics: canonical transformations and Riemannian ge-

ometry. In: IEEE International Conference on Robotics and Automation, Nice, France, pp.554–559 (1992)

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

[173] Sundials: Initial Value Problems. Website, http://sundials.wikidot.com/ivp-problems (2010)

http://sundials.wikidot.com/ivp-problems

482 References

[174] Tasora, A., Negrut, D., Anitescu, M.: Large-scale parallel multibody dynamics withfrictional contact on the graphical processing unit. J. Multibody Syst. Dyn. 222(K4),315–326 (2008)

[175] Trinkle, J.: Formulation of multibody dynamics as coplementarity problems. In: ASMEInternational Design Engineering Technical Conference, Chicago, IL (2003)

[176] Umetani, Y., Yoshida, K.: Resolved motion rate contol of space manipulators with general-ized Jacobian matrix. IEEE Trans. Robot. Autom. 5(3), 303–314 (1989)

[177] Vafa, Z.: Space manipulator motions with no satellite attitude disturbances. In: IEEE Inter-national Conference on Robotics and Automation, Cincinnati, OH (1990)

[178] Vereshchagin, A.: Computer simulation of the dynamics of complicated mechanisms ofrobot-manipulators. Eng. Cybern. 6, 65–70 (1974)

[179] Walker, M., Orin, D.: Efficient dynamic computer simulation of robotic mechanisms. ASMEJ. Dyn. Syst. Meas. Contr. 104(3), 205–211 (1982)

[180] Wen, J., Bayard, D.: A new class of control laws for robotic manipulators. Int. J. Contr. 47,1361–1385 (1988)

[181] Wen, J., Kreutz, K.: A control perspective for multiple arm systems. In: 2nd InternationalSymposium on Robotics and Manufacturing, Albuquerque, NM (1988)

[182] Wen, J., Kreutz, K., Bayard, D.: A new class of energy based control laws for revolute arms.In: American Control Conference, Atlanta, GA (1988)

[183] West, D.B.: Introduction to Graph Theory. Prentice-Hall, Englewood Cliffs, NJ (2001)[184] Wikipedia: Tree (graph theory). http://en.wikipedia.org/w/index.php?title=Tree (graph

theory)&oldid=327865637 (2009)[185] Wittenburg, J.: Dynamics of Systems of Rigid Bodies. BG Teubner, Stuttgart (1977)[186] Yamane, K., Nakamura, Y.: Dynamics computation of structure-varying kinematic chains

and its application to human figures. IEEE Trans. Robot. Autom. 16(2), 124–134 (2000)[187] Yamane, K., Nakamura, Y.: Dynamics simulation of humanoid robots: Forward dynamics,

contact, and experiments. In: CISM-IFToMM Symposium on Robot Design, Dynamics, andControl (ROMANSY2008), Tokyo, Japan, pp. 301–308 (2008)

[188] Yamane, K., Nakamura, Y.: Robotic Science and Systems IV, The MIT Press, chap A nu-merically robust LCP solver for simulating articulated rigid bodies in contact, pp. 89–104(2009)

[189] Zanganeh, K., Angeles, J.: A formalism for the analysis and design of modular kinematicstructures. Int. J. Robot. Res. 17(7), 720–730 (1998)

http://en.wikipedia.org/w/index.php?title=Tree_(graph_theory)&oldid=327865637

http://en.wikipedia.org/w/index.php?title=Tree_(graph_theory)&oldid=327865637

List of Notation

SymbolsXω the spatial vector with just the angular component of the

X spatial vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9Xv the spatial vector with just the linear component of the X

spatial vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

XS = col{X(℘(k))

}nk=1

– a shifted version of a X stacked

vector with the kth slot being occupied by the X(℘(k))

element . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353[ij,k] Christoffel symbols of the first kind . . . . . . . . . . . . . . . . . . . 381{kij

}Christoffel symbols of the second kind . . . . . . . . . . . . . . . . . 381

col{x(i)

}pi=m

the stacked vector consisting of the x(i) elements for bod-

iesm through p in the multibody system . . . . . . . . . . . . . . . 48Fx representation of vector x in frame F . . . . . . . . . . . . . . . . . . 4I the identity matrix of appropriate dimension . . . . . . . . . . . . 12

1[set] indicator function that returns a 1 if the element belongsto the set, and 0 otherwise . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

∇θf(θ) the gradient of a the vector-valued function f with respectto the θ vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401

z an operator related to the cross-product operator for spa-tial vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

˜l cross product operator l⊗ (·) . . . . . . . . . . . . . . . . . . . . . . . . . 6˜X = diag

{˜X(k)

}nk=1

– a block-diagonal matrix with ˜X(k)

diagonal elements from the X stacked vector . . . . . . . . . . . . 353{l}

∼ same as ˜l . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

X = diag{X(k)

}nk=1

– block-diagonal matrix with X(k)

diagonal elements from the X stacked vector . . . . . . . . . . . . 353x = − { x}

∗ – cross-product related operator for spatial vectors 10

483

484 List of Notation

0 the zero matrix of appropriate dimension . . . . . . . . . . . . . . . 9i⊀ j node j is not the ancestor of node i in a digraph . . . . . . . . . 136i≺ j node j is the ancestor of node i in a digraph . . . . . . . . . . . . . 136j� i node j is not the ancestor of node i in a digraph . . . . . . . . . 136j� i node j is the ancestor of node i in a digraph . . . . . . . . . . . . . 136

Aa(k) the Coriolis spatial acceleration for the kth link . . . . . . . . . 77

aB(k) the Coriolis acceleration for the kth body in the serial-chain equations of motion with body spatial acceleration,defined as the body frame derivative of the spatial veloc-ity of the body frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

aI(k) the Coriolis acceleration for the kth body in the serial-chain equations of motion with body spatial accelerationsdefined as the inertial frame derivative of the spatial ve-locity of the body frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

aS the stacked sub-vector of Coriolis spatial accelerationvectors for the S sub-graph bodies . . . . . . . . . . . . . . . . . . . . 294

aI(k) the Coriolis spatial acceleration for the kth body withinertially referenced spatial accelerations . . . . . . . . . . . . . . . 94

α+(k) = φ∗(k+ 1,k)α(k+ 1) ∈ R6 - the rigidly propagatedα(k+ 1) (body-frame derivative) spatial accelerationfrom the Ok+1 frame to the O

+k

frame . . . . . . . . . . . . . . . . . 101αI(k) ∈ R6, spatial acceleration of the kth link referred to

frame I . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94A = (I−EA)−1 – a generic SPO operator . . . . . . . . . . . . . . . . 144

Aa the SPO operator for the aggregated tree . . . . . . . . . . . . . . . 289AC the SPO operator for the C child sub-graph . . . . . . . . . . . . . 276AP the SPO operator for the P parent sub-graph . . . . . . . . . . . . 276AS the SPO operator for a S sub-graph . . . . . . . . . . . . . . . . . . . 276

A = A−I – a spatial operator derived from a generic A SPOoperator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146

Bb(k) the gyroscopic spatial force for the kth link . . . . . . . . . . . . 76

bI(z) the gyroscopic spatial force for the equations of motionof a rigid body using inertial frame derivatives . . . . . . . . . . 26

bS the stacked sub-vector corresponding to the bodies in theS sub-graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295

β(k) the vector of generalized velocity coordinates for the kthhinge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

βB = BV – the rigid body generalized velocity coordinatesdefined as the body frame representation of the body’sspatial velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

List of Notation 485

βI = IV – the rigid body generalized velocity coordinatesdefined as the inertial frame representation of the body’sspatial velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

βIIVI – the rigid body generalized velocity coordinates de-fined as the inertially referenced spatial velocity of the body 33

bI the gyroscopic spatial force associated with the equationsof motion about an inertially fixed velocity reference point 33

B a body-fixed coordinate frame . . . . . . . . . . . . . . . . . . . . . . . . 23Bk the reference frame for the kth link . . . . . . . . . . . . . . . . . . . 36B ∈R6n×6nnd – the pick-off operator for body nodes . . . . . 55

BS the connector block whose non-zero elements define theparent/child connectivity between links in the S sub-graph and their children in the C child sub-graph . . . . . . . . 276

C�(k) the set of immediate child nodes of the kth node in a

digraph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136C the induced sub-graph consisting of the descendant nodes

of the S sub-graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274C(θ, θ) the system-level Coriolis and gyroscopic forces vector . . . . 67Ci(j,k) Christoffel symbol of the first kind . . . . . . . . . . . . . . . . . . . . 68

C the location of the center of mass for a rigid body . . . . . . . . 19

DdFxd∗ derivative of vector x(s) with respect to s in frame F . . . . . 5

D(k) H(k)P(k)H∗(k) – the articulated body hinge inertia forthe kth body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

ΔB/O = diag{φ(Bk,Ok)

}– the Bk body frame to the Ok

transformation operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83VB/O(k) the relative spatial velocity of the Ok frame with respect

to the Bk frame on the kth body . . . . . . . . . . . . . . . . . . . . . . 359

ΔωV = col{ΔωV (k)

}nk=1

– the stacked vector of angular rela-

tive hinge spatial velocities for all the links . . . . . . . . . . . . . 354

ΔvV = col{ΔvV(k)

}nk=1

– the stacked vector of linear relative

hinge spatial velocities for all the links . . . . . . . . . . . . . . . . . 354Δv(k) the relative linear velocity across the kth hinge . . . . . . . . . 38ΔV(k) relative spatial velocity across the kth hinge . . . . . . . . . . . . 39Δω(k) the relative angular velocity across the kth hinge . . . . . . . . 38ΔB

V(k) the hinge relative spatial velocityΔV(k) referenced to thekth link body frame Bk . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

ΔI

V(k) the spatial velocity across the kth hinge in the inertiallyreferenced formulation of the equations of motion . . . . . . . 47


Eε(k) the articulated body inertia innovations generalized force . 111

ek ∈ RN×mk – a n elements block-vector with all zero el-ements except for the kth element which is the identitymatrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

Eφ the SKO operator for rigid-link multibody systems . . . . . . . 48EφB

=ΔB/OEφΔ−1B/O

– the SKO operator for rigid-link multi-body systems for the case when Bk �= Ok . . . . . . . . . . . . . . 83

EACthe SKO operator for the C child sub-graph . . . . . . . . . . . . . 275

EAPthe SKO operator for the P parent sub-graph . . . . . . . . . . . . 275

EASthe SKO operator for the S sub-graph . . . . . . . . . . . . . . . . . 275

EAathe SKO operator for the aggregated tree . . . . . . . . . . . . . . . 288

EA a generic SKO operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144E a transformed version of the E base pick-off operator . . . . . 87E the base pick-off operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

ES the connector block whose non-zero elements define theparent/child connectivity between links in S and theirparents in the P sub-graph . . . . . . . . . . . . . . . . . . . . . . . . . . . 276

Eψ = Eφτ – the articulated body SKO operator . . . . . . . . . . . . 116

Ff(F) spatial force at the F frame . . . . . . . . . . . . . . . . . . . . . . . . . . . 15f(k) the spatial force of interaction between the (k+1)th and

the kth links . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75fc the constraint spatial forces being applied at nodes on the

multibody system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131fic(k) the constraint spatial force at the ith node on the kth body 131fext the stacked vector of external spatial forces on the system . 85

fiext(k) the external spatial forces on the ith node on the kth body 85fI the equivalent spatial force on a rigid body associated

with an inertially fixed velocity reference point . . . . . . . . . . 32fI(k) ∈ R6 – the spatial force of interaction between the

(k+ 1)th and the kth link referred to frame I . . . . . . . . . . . 94

Gg the gravity spatial acceleration vector . . . . . . . . . . . . . . . . . . 87

gl the gravity linear acceleration vector . . . . . . . . . . . . . . . . . . . 87G(k) P(k)H∗(k)D−1(k) – the articulated body Kalman gain

operator for the kth body . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

Hhω(k) the angular sub-block of the joint map matrix H∗(k) for

the kth body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40


hv(k) the linear sub-block of the joint map matrixH∗(k) for thekth body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

H∗(k) the hinge map matrix for the kth hinge . . . . . . . . . . . . . . . . 39H the block-diagonal spatial operator of hinge map matrices . 49HB = HΔ−1

B/O– the block-diagonal spatial operator of hinge

map matrices when Bk �= Ok . . . . . . . . . . . . . . . . . . . . . . . . . 84HI the block-diagonal spatial operator of hinge map matrices

for inertially referenced formulation of the equations ofmotion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

HC the H operator for the C child sub-graph . . . . . . . . . . . . . . . 291HP the H operator for the P parent sub-graph . . . . . . . . . . . . . . 291HS the H operator for the S sub-graph . . . . . . . . . . . . . . . . . . . . 291Ha the H operator for the aggregated tree . . . . . . . . . . . . . . . . . . 291

H=i = col{H∗(i) · 1[k=i]

}nk=1

- the derivative of ΔV with

respect to θi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363

Hω=i = col

{H∗ω(i) ·1[k=i]

}nk=1

- the derivative of ΔωV with

respect to θi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363

Hω�i = col

{H∗ω(i) ·1[k�i]

}nk=1

- the derivative of Vω with

respect to θi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363

Hω≺i = col

{H∗ω(i) ·1[k≺i]

}nk=1

- the derivative of VωS

with

respect to θi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363

Hv=i = col

{H∗v(i) · 1[k=i]

}nk=1

- the derivative of Vv

with respect to θi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363h(z) spatial momentum of a rigid body . . . . . . . . . . . . . . . . . . . . . 21

II an inertially-fixed coordinate frame . . . . . . . . . . . . . . . . . . . 23

JJ ∈R6nnd×N – the Jacobian matrix . . . . . . . . . . . . . . . . . . . . 55

J (k) rotational inertia of a rigid body . . . . . . . . . . . . . . . . . . . . . . 18

KK = EφG – the spatial operator formed from the shifted

Kalman gain elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116K(k+ 1,k) = φ(k+ 1,k)G(k) – the shifted Kalman gain operator . . . . 108

Ke the system kinetic energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

LP+ = P+ + Δ

v

V P+ − P+˜ΔvV - an operator associated with

the time derivative of the articulated body inertia, P . . . . . . 371


λ the forward Lyapunov equation solution associated withthe time derivative of the P articulated body inertia. . . . . . . 368

λθi = ∂λ

∂θi– an intermediate spatial operator used for the

computation of the sensitivity of the P articulated bodyinertia spatial operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373

l(F,G) vector from frame F to frame G . . . . . . . . . . . . . . . . . . . . . . 3Λ operational space inertia matrix . . . . . . . . . . . . . . . . . . . . . . . 188Λ the operational space compliance matrix . . . . . . . . . . . . . . . 188L the Lagrangian function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

Mm mass of a rigid body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

MD(θ, θ) the gradient of the generalized momentum . . . . . . . . . . . . . . 67M the mass matrix of a multibody system . . . . . . . . . . . . . . . . . 67

MB = ΔB/OMΔ∗B/O

– the block-diagonal spatial operator ofbody spatial inertias about the body frame when Bk �= Ok 84

M(k) the spatial inertia of the kth link . . . . . . . . . . . . . . . . . . . . . . 57M(x) spatial inertia of a rigid body referenced to point x . . . . . . . 18

MI the block-diagonal spatial operator with inertially refer-enced body spatial inertias along the diagonal . . . . . . . . . . . 95

MI

�= φ(I,C)Mφ∗(I,C) ∈ R6×6, the inertially referencedspatial inertia of a body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

MS rigid body spatial inertia matrix for the S sub-graph . . . . . 295

Nn the number of bodies in the multibody system . . . . . . . . . . . 35nG the number of bodies in the S sub-graph . . . . . . . . . . . . . . . 344

nnd(k) number of nodes on the kth body . . . . . . . . . . . . . . . . . . . . . 54nnd the number of nodes on the system . . . . . . . . . . . . . . . . . . . . 54ν(k) the articulated body inertia innovations generalized accel-

eration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111ν = ν−G∗E∗g – the articulated body innovations acceler-

ation with gravity contribution included . . . . . . . . . . . . . . . . 129N the total number of velocity degrees of freedom for the

system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

OOik the ith node on the kth links . . . . . . . . . . . . . . . . . . . . . . . . 54

ω(x) the angular velocity of the x frame . . . . . . . . . . . . . . . . . . . . 6ω(F,G) the angular velocity of the G frame with respect to the F

frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13Ω the extended operational space compliance matrix . . . . . . . 190

Ok outboard hinge reference frame for the kth link . . . . . . . . . 35O

+k inboard hinge reference frame for the kth link . . . . . . . . . . 35


P℘(k) the set of parent nodes of the kth node in a digraph . . . . . . 136

P the induced sub-graph for the nodes not in, or descendantof, the S sub-graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274

p(k) the 3-vector from the point k to the center of mass of arigid body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

φ = (I−Eφ)−1 – the SPO operator for rigid-link multibodysystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

φ(x,y) rigid body transformation matrix for the x and y frames . . 11φB = ΔB/OφΔ

−1B/O

– the SPO operator for the case whenBk �= Ok . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

φ(k,k− 1) the rigid body transformation matrix from frame Bk toframe Bk−1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

φ =φ−I – the strictly lower-triangular spatial operator de-rived from the φ SPO operator . . . . . . . . . . . . . . . . . . . . . . . 51

ψ = (I−Eψ)−1 – the articulated body SPO operator . . . . . . . 117ψ(k+ 1,k) = φ(k+ 1,k)τ(k) – the articulated body transformation

matrix for the kth body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105ψ = ψ− I – the articulated body spatial operator derived

from the ψ SPO operator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119P(k) the articulated body inertia of the kth link . . . . . . . . . . . . . . 99

P = EφP+E∗φ - an operator associated with the time deriva-

tive of the articulated body inertia, P . . . . . . . . . . . . . . . . . . 371

P+(k)�= P(k)τ∗(k) the articulated body inertia P(k) trans-formed across the joint from Ok to Ok+1 for the kthbody . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

P+ = τλτ∗ - an operator associated with the time derivativeof the articulated body inertia, P . . . . . . . . . . . . . . . . . . . . . . 371

Rrp(k) number of hinge generalized coordinates for the kth hinge 37rv(k) the number of generalized velocity coordinates for the

kth hinge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39R(k) the composite rigid body inertia associated with the kth

hinge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59R the block-diagonal spatial operator consisting of compos-

ite body inertia of the links . . . . . . . . . . . . . . . . . . . . . . . . . . . 61FRG ∈ R3×3 – a rotation matrix that transforms vector repre-

sentations from the G frame to the F frame. . . . . . . . . . . . . . 4

SS the adjacency matrix for digraphs . . . . . . . . . . . . . . . . . . . . . 138

SW a BWA matrix for a tree digraph . . . . . . . . . . . . . . . . . . . . . . 141


Tθ the vector of generalized coordinates for the system . . . . . . 48

τ(k) the articulated body projection operator for the kth hinge . 102τ(k) = I−τ(k) – the complement of projection operator τ(k)

for the kth hinge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102T(k) the generalized force vector for the kth hinge . . . . . . . . . . . 77FTG homogeneous transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4T a rooted directed tree of nodes and edges . . . . . . . . . . . . . . . 137

UU the projection operator for operational space dynamics . . . 199

U⊥ = I−U – the complement of the U projection operatorsfor operational space dynamics . . . . . . . . . . . . . . . . . . . . . . . 199

Υ+ matrix associated with Υ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194Υ kernel matrix associated with operational space inertias . . . 191

VvF(F,G) the linear velocity of the G frame with respect to the F

frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14v(x) linear velocity vector for point x . . . . . . . . . . . . . . . . . . . . . . 7vI the linear velocity of the inertially fixed velocity refer-

ence frame I for a rigid body . . . . . . . . . . . . . . . . . . . . . . . . . 31vI(k) the linear velocity of the inertially fixed velocity refer-

ence frame I for the kth body . . . . . . . . . . . . . . . . . . . . . . . . 93

Vω = col{

Vω(k)

}nk=1

– the stacked vector of angular spa-

tial velocities for all the links . . . . . . . . . . . . . . . . . . . . . . . . . 354Vω(F) the angular spatial velocity component of the V(F) spatial

velocity of F frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9Vnd =∈R6nnd – the stacked vector of spatial velocities of the

task space nodes (also used as generalized velocities forOperational Space dynamics) . . . . . . . . . . . . . . . . . . . . . . . . . 54

VF(F,G) the spatial velocity of the G frame with respect to the F

frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14Vv = col

{Vv(k)

}nk=1

d – the stacked vector of linear spatial

velocities for all the links . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354Vv(F) the linear spatial velocity component of the V(F) spatial

velocity of F frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9V(

Oik

) ∈R6 – the spatial velocity of the ith node on the kth links 54V the stacked vector of link spatial velocities . . . . . . . . . . . . . 48

V(F) spatial velocity of the F frame . . . . . . . . . . . . . . . . . . . . . . . . 9V(k) spatial velocity of the Bk frame . . . . . . . . . . . . . . . . . . . . . . . 43

V+(k) spatial velocity of O+k frame . . . . . . . . . . . . . . . . . . . . . . . . . 43


˜Vω(z) a ∈R6×6 cross-product matrix associated with the Vω(z)

angular spatial velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25˜Vω

S= diag

{˜Vω(℘(k))

}nk=1

– the block-diagonal matrix

with ˜Vω(℘(k)) elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354

Vv

S = diag{

Vv(℘(k))

}nk=1

– the block-diagonal matrix with

Vv(℘(k) elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354

V = diag{

V(k)

}– the block-diagonal spatial operator with

V(k) diagonal elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354

VS = diag{

V(℘(k))}

– the block-diagonal, shifted up ver-

sion of the V spatial operator . . . . . . . . . . . . . . . . . . . . . . . . . 354VS the stacked sub-vector corresponding to the S sub-graph

bodies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 290VI the inertially referenced spatial velocity of a point on a

body, with ω and vI denoting the angular and linear ve-locity components, respectively . . . . . . . . . . . . . . . . . . . . . . . 31

VI(k) the spatial velocity of the kth link referred to frame I,with ω(k) and u(k) denoting the angular and linear ve-locity components, respectively . . . . . . . . . . . . . . . . . . . . . . . 93

Zz(k) the residual spatial force for the kth link . . . . . . . . . . . . . . . 107

z+(k) the z(k) articulated body inertia residual force propagatedacross the kth hinge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

zδ the correction residual spatial force due to a non-zero tipforce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131

Index

Symbolsφ(·, ·), 11

group property, 11E+ matrix, 408E− matrix, 408operator, 10

identities, 12˜cross-product operator

for 3-vectors, 6for spatial vectors, 9

kth articulated body system, 1001-resolvent

BWA matrix, 143SKO operator, 159SPO operator, 159elements, 144of a nilpotent matrix, 400

AAB forward dynamics

SKO models, 182flexible-link systems, 264, 267for free-flying dual model, 335free-flying systems, 338, 339geared systems, 243optimized algorithm for geared

systems, 242serial-chain systems, 126, 130standard algorithm for geared

systems, 241tree-topology systems, 182under-actuated systems, 322with constraint embedding, 308

absolute coordinates, 38acatastatic constraint, 39, 210active

degree of freedom, 314arm, 315hinge, 314

active contact, 225acyclic digraph (DAG), 137adjacency matrix, 138

BWA, see BWA matrixblock-weighted, see BWA matrixcanonical tree, 140digraph, 138lower-triangular, 140properties, 138serial-chain, 140strictly canonical tree, 140tree, 140

adjacent node, 135aggregation

SPO operator, 289at the component level, 292condition, see aggregation

conditionfor sub-structuring SKO models,

290mass matrix, 291of a sub-graph, 283of an induced sub-graph, 283preservation of tree structure, 285sub-graph, 287

aggregation condition, 284, 299path-induced sub-graphs, 285preservation of tree property, 287transformed SKO model, 291

algorithm for computingφx recursively, 51φ∗x recursively, 52θδ correction accelerations, 131Ax recursively, 162A∗x recursively, 163Υ operational space compliance

kernel, 195extended operational space

compliance matrix, 192

493

494 Index

operational space compliancekernel, 195

articulated body inertia, seearticulated body inertia

composite body inertia, seecomposite body inertia

diagonalizing generalized forces,386

diagonalizing velocitycoordinates, 386

disturbance Jacobian, 328dual

articulated body inertias, 334residual forces, 335

elements of AXB∗, 168

elements of A∗XB, 173

forward dynamics, see forwarddynamics

generalized dynamics, 322generalized forces for external

forces, 86generalized Jacobian, 326inverse dynamics, see inverse

dynamicslink spatial velocities, 45link transformations, 43mass matrix

inverse, 198using composite body inertias,

64, 169, 238, 261using inverse dynamics, 90

operational spaceCoriolis vector, 203inertia, 194, 351

under-actuated computed-torque,326

velocity diagonalization Coriolisvector, 391

algoritm for computingmass matrix inverse, 200

ancestor node, 135angle/axis representation,

405angular velocity, 5

based quasi-velocities, 71

from angle/axis parameter rates,406

from Euler angle rates, 404from quaternion rates, 411to quaternion rates, 410

anti-causal filter, 112, 338arborescence digraph, 137articulated bodykth system, 100dual type, 333forward dynamics, see AB

forward dynamicshinge inertia, 101inertia, see articulated body inertiamodel, see articulated body modeltime-derivative, 368–373transformation matrix, 105

articulated body inertia, 99, 117relationship to spatial inertias, 107for SKO models, 175for flexible-link systems, 262for geared systems, 239, 240for under-actuated systems, 317force decompositions based on, 99positive semi-definiteness of, 104relationship to composite body

inertias, 107, 121serial-chain systems, 106with constraint embedding, 307

articulated body model, 97, 99–111,115

based force decompositions, 128comparison with the composite

body model, 117projection operators for, 102

assumed modes, 250attitude representation, 23

angle/axis parameters, 404Euler angles, 403Gibbs vector, 412Rodrigues vector, 412unit quaternions, see quaternions

augmented dynamics, 212equivalence to projected

dynamics, 222

Index 495

augmented forward dynamics,212–221

algorithm, 213, 219loop constraints, 215

Bbackward Lyapunov equation, 170,

191base-body, 35base-invariant

computation of operational spaceinertia, 351

forward dynamics, 338, 339operational space inertia, 348symmetry, 338

base-to-tip recursion, 52base-to-tips scatter recursion, 163bilateral constraint, 209block-weighted adjacency matrix,

see BWA matrixbody reference frame, 36

at hinge frame, 45, 77at non-hinge frame, 46, 80, 358

BWA matrix, 141–149SKO operator, 159SPO operator, 1591-resolvent, 143for multibody systems, 150–159nilpotency, 143permutation transformation, 148powers of, 142similarity-shift transformation,

149transforming, 147tree digraph, 142

Ccanonical tree digraph, 137

adjacency matrix, 140cantilever modes, 250catastatic constraint, 39, 210, 301causal filter, 112, 338center of mass, 19

equations of motion about, 25of a multibody system, 62

of a rigid body, 18spatial velocity, 63

centeroid, 19characteristic polynomial

of a rotation matrix, 405of a skew-symmetric matrix, 405

child node, 135child nodes of a sub-graph, 284child sub-graph, 274Christoffel symbols

for multibody systems, 69of the first kind, 68, 381of the second kind, 381relationship toMD(i, j), 69relationship to Coriolis terms, 69

closed differential form, 71closed-chain dynamics, 150,

209–227, 297SKO model, 185, 299augmented approach, see

augmented dynamicsbilateral constraints, 209constraint embedding approach,

see constraint embeddingdynamics

direct approach, see directdynamics

projected approach, see projecteddynamics

SKO model, 302unilateral constraints, 224

compatible operators, 162compatible operators and vectors,

162complementarity

approach, 224condition, 226problem

LCP, 226NCP, 226

composite body inertia, 59–66, 98,107

based computation of the massmatrix, 238, 261

for flexible-link systems, 261

496 Index

for geared systems, 237for inverse dynamics, 91Lyapunov equation for, 61, 168recursive computation of

for SKO models, 169for flexible-link systems, 261for geared systems, 237for serial-chain systems, 60

relationship to articulated bodyinertias, 121

serial-chain systems, 59composite body method, 65composite body model, 92, 97, 98,

115comparison with the articulated

body model, 117computed-torque for under-actuated

systems, 326configuration kinematics, 42connected digraph, 136conservation of

kinetic energy, 28, 30, 329spatial momentum, 25, 28, 33, 329

constrained dynamics, seeclosed-chain dynamics

constraintacatastatic, 39, 210bilateral, 209catastatic, 39, 210force, 222

squeeze, 214holonomic, see holonomic

constraintloop, 215non-holonomic, see

non-holonomic constraintrheonomic, 210scleronomic, 210unilateral, see unilateral constraint

constraint embedding, 212, 297–311AB forward dynamics, 308direct joint-level constraints, 303four-bar linkage example, 305,

306geared example, 304

generalizations, 311loop constraints, 303SKO model, 302strategy, 297

constraint forces, 131closed-chain, 218inter-link, see inter-link spatial

forcescontact

active, 225inactive, 225separation, 225

contact hinge, 41control

computed-torque, 326decoupled, 392operational space, 189rate-feedback, 392

coordinate transformations, 72diagonalizing, 72, 379–382with kth link as base-body, 341

coordinate–frame representations, 4coordinate–free representations, 4coordinates

generalized, see generalizedcoordinates

generalized velocity, seegeneralized velocitycoordinates

Coriolis acceleration, 77, 358, 434dual, 335for a cylindrical hinge, 80for a helical hinge, 80for a prismatic hinge, 79for a rotary pin hinge, 79for simple hinges, 79with hinge frame as body frame,

77with inertial frame derivatives, 80with non-hinge frame as body

frame, 80Coriolis forces vector, 67

for SKO models, 160for diagonalized dynamics, 382,

387, 393

Index 497

for flexible-link systems, 257for geared systems, 236for serial-chain rigid-link system,

82Lagrangian form, 67non-working, 384, 391operational space, 202with velocity diagonalization, 389

algorithm for computing, 391correction generalized accelerations,

212cross product

for 3-vectors, 6for spatial vectors, 9–10identities, 10, 397

curvature tensor, 381cut-edge, 138, 209

multibody, 151cyclic digraph, 137cylindrical hinge, 40, 41

Coriolis acceleration, 80

DDAG, 137DCA algorithm, 218decomposition of the mass matrix

using composite body inertias, 63,168

decoupled control, 392decoupled forward dynamics, 338,

339derivative

with respect to a vector, 401with respect to generalized

coordinates, see sensitivitywith respect to time, see time

derivativeof a vector

with respect to a frame, 5with respect to the body frame, 7with respect to the inertial frame,

7of spatial inertias, 25, 32of spatial operators, 353of vectors, 5–8

time, see time derivativedescendant node, 135determinant

of the mass matrix, 180of the Newton-Euler factor, 180

diagonalized dynamicscondition, 379, 382Coriolis forces vector, 382, 387,

393diagonalizing coordinate

transformations, 72algorithm for computing, 386global, 379–382velocity, 382–392

diagonalizing generalized forcesalgorithm for computing, 386

differential equationdifferential-algebraic, see

differential-algebraic equationordinary, see ordinary differential

equationdifferential kinematics, 43

hinge, 37with Bk �= Ok, 45

differential-algebraic equation, 213,222, 339

digraphacyclic graph (DAG), 137adjacency matrix, see adjacency

matrixadjacent nodes, 135aggregation condition, 284aggregation of a sub-graph, 283arborescence, 137connected, 136cut-edge, 138, 209cyclic, 137, 209disconnected, 136edge, 135edge-contraction, 283for a multibody system, see

multibody digraphforest, 137homeomorphic, 279multiply-connected, 137, 209

498 Index

node, 135node-contraction, 283partitioning, 273–275polytree, 137properties, 141related nodes, see related nodesroot node, 135rooted, 136serial-chain, 137simple tree, 137simply-connected, 137spanning tree, 138standard, 150tree, 137unrelated nodes, see unrelated

nodesdirect dynamics, 212directed acyclic graph, 137directed cycle, 209

in path-induced sub-graph, 274directed graph, see digraphdirection cosine matrix, see rotation

matrixdisconnected digraph, 136distance function, 225disturbance Jacobian, 325

computation of, 328of free-flying systems, 328, 330

divide and conquer algorithm,208

dualarticulated body inertia, 334Coriolis acceleration, 335force decomposition, 336model, 332residual force, 335weight matrices, 333

dynamicsaugmented dynamics, 212closed-chain systems, 209constraint embedding dynamics,

212diagonalized, 379direct dynamics, 212flexible-link systems, 245

for SKO models, 160free-flying systems, 331geared systems, 229hybrid, 324invariance across Newtonian

frames, 30, 82operational space, 189prescribed motion, 322projected dynamics, 212rigid body systems, 17serial-chain systems, 75tree-topology systems, 155under-actuated systems, 313

Eedge, 135

multibody, 150edge-contraction, 283equations of motion

for SKO models, 160for flexible-link systems, 245–255for geared systems, 230–234for tree-topology systems, 155for under-actuated systems, 316Hamilton form, 70Lagrangian form of, see

Lagrangian dynamicsoperational space, 187, 189rigid body, 25

Euclidean metric tensor, 381Euler angles, 41, 403Euler equation for rotational motion,

71Euler parameters, see quaternionsEuler–Rodrigues formula, 405explicit hinge constraints, 36extended operational space

compliance matrix, 190algorithm for computing, 192operator decomposition, 191

for serial-chains, 197external force

compensating for, 86equivalent generalized forces, 85inclusion of, 85, 129

Index 499

Ffeedback linearization, 189Fixman potential, 376flexible joints, 233flexible-link system

SKO models, 255–257articulated body inertia, 262as under-actuated systems, 261composite body inertias, 261Coriolis forces vector, 257equations of motion, 245–255forward dynamics, 264, 267Innovations operator factorization,

263inverse dynamics for, 259lumped mass model, 245–249mass matrix, 257

decomposition, 261recursive computation of, 261

modal model, 249–255force

constraint, 222move, 214squeeze, 214

force decompositiondual model, 336using articulated body model, 99,

128using composite body model, 98using terminal body model, 98

force recursionfor aggregated bodies, 295for serial-chains, 76for tree systems, 156

forest digraph, 137, 147forward dynamics, 88

AB algorithm, see AB forwarddynamics

O(N) algorithm, see AB forwarddynamics

O(N3) algorithm, 122O(N2) algorithm, 122articulated body model, 97augmented dynamics algorithm,

213

base-invariant, 338DCA algorithm, 208decoupled form, 338, 339diagonalized dynamics, 394divide and conquer algorithm, 208flexible-link systems, 264for SKO models, 182free-flying systems, 337geared systems, 239, 243of under-actuated systems, 322projected dynamics algorithm, 223un-normalized diagonalized

dynamics, 395with bilateral constraints, 211with loop constraints, 215, 219with unilateral constraints, 227

forward kinematics, 42forward Lyapunov equation, 370

for SPO operators, 166frame

body reference, 36hinge, 37Newtonian, 30, 82of interest, see node

frame of interest, 54free generalized accelerations, 212free-flying system, 331

as under-actuated system,329–330

base-invariant symmetry, 338disturbance Jacobian, 328, 330dual articulated body inertias, 333dual model, 332forward dynamics, 337–339generalized Jacobian, 325, 328regular model, 332with tree topology, 340

free-free modes, 250friction, 227

Ggap function, 225gather recursion, 162geared system, 229

articulated body inertia, 239

500 Index

articulated body inertias, 240composite body inertia, 237Coriolis forces vector, 236equations of motion, 230–234forward dynamics, 239, 243Innovations operator factorization,

240mass matrix, 236

decomposition, 237recursive computation of, 238

optimized AB forward dynamics,242

standard AB forward dynamics,241

generalized accelerations, 24correction, 212free, 212inertial, 27innovations, 111

generalized coordinates, 23, 37absolute, 38diagonalizing transformation, 379for flexible-links, 251for geared systems, 233hinge, 37minimal, 212non-minimal, 212relative, 38transformation, 72

generalized dynamics algorithm, 322generalized forces, 24

for external forces, 85generalized inverse, 214generalized Jacobian, 325

computation of, 326of free-flying systems, 325, 328

generalized momentum, 67gradient of, 67, 365

generalized speeds, see generalizedvelocity coordinates

generalized velocity coordinates, 23,39

diagonalizing transformation, 382for flexible-links, 251for geared systems, 233, 234

hinge, 39quasi-velocities, 23transformation, 70

Gibbs vector attitude representation,412

global diagonalization condition,379

gradientwith respect to a vector, 401of the generalized momentum, 67,

365graph

directed, see digraphundirected, 135

gravitational force, 87, 128, 202group

of homogeneous transforms, 4of rigid body transformation

matrices, 11gyroscopic force, 75

using βB , 30using βI, 26, 28

HHamilton form of equations of

motion, 70helical hinge, 40, 41

Coriolis acceleration, 80hinge, 35

articulated body inertia, 101contact, 41cylindrical, 40, 41, 80explicit constraints, 36frame, 37helical, 41, 80implicit constraints, 36kinematics, 37prismatic, 40rotary pin, 40spherical, 40, 41universal, 41

hinge map matrix, 39configuration dependent, 41cylindrical hinge, 40prismatic hinge, 40

Index 501

rotary pin hinge, 40spherical hinge, 40

holonomic constraint, 210homeomorphic digraphs, 279homogeneous transform, 4, 37, 42homogenous transform

time derivative, 38hybrid dynamics, 324

Iignorable coordinates, 329implicit hinge constraints, 36inactive contact, 225inboard link, 35indicator function, 138, 362induced sub-graph, 273inertia

articulated body, 99composite body, 59moments of, 18rotational, 18spatial, 18

inertial generalized accelerations, 27inertially fixed reference point, 47

based velocity recursion, 47for a rigid body, 31–33for serial-chains, 93

innovations generalized acceleration,111

innovations generalized force, 111Innovations operator factorization,

58, 120after constraint embedding, 308for SKO models, 179for flexible-link systems, 263for geared systems, 240for serial-chain systems, 115, 120,

122for tree-topology systems, 179for under-actuated systems, 319of the mass matrix, 115, 120sensitivity, 375time derivative, 375

innovations process, 113integrals of motion, 34, 329

inter-link spatial forces, 75articulated-body model

decomposition, 99composite-body model

decomposition, 98computation, 128operator decomposition, 128terminal-body model

decomposition, 97inverse dynamics, 88

for computing the mass matrix, 90for flexible-link systems, 260for serial-chains, 88–93for under-actuated systems, 322Newton–Euler algorithm, 89Newton-Euler algorithm, 166of flexible-link systems, 259of under-actuated systems, 322using composite rigid body

inertias, 91inverse kinematics, 42

JJacobi cross-product identity, 10Jacobian

disturbance, 325for under-actuated systems,

324–328generalized, 325in operational space dynamics,

188mapping for external forces, 86operator, 55

joint, see hinge

KKalman gain, 101, 113

shifted, 108Kane’s method, 161kinematics

differential, 43forward, 42hinge, 37inverse, 42

kinetic energy, 18as Lagrangian, 67

502 Index

conservation, 28, 30, 329flexible body, 251integral of motion, 329rate of change of, 384sensitivity of, 366serial-chain, 57using diagonalizing coordinates,

380

LLagrange multipliers, 211, 222Lagrangian dynamics

Coriolis forces vector, 67equations of motion, 66–70equivalence with Newton–Euler

dynamics, 367symmetries of, 34, 329

LCP, 226linear complementarity

problem, 226linear velocity constraints, 36link, 35

base-body, 35flexible, 245inboard, 35outboard, 35tip-body, 35

link transformations, 43load balancing, 215loop constraints, 215, 303lower-triangular

SKO matrix, 146SPO matrix, 145, 146

lower-triangular adjacency matrix,140

lumped mass model for flexiblesystems, 245–249

Lyapunov equationbackward, see backward

Lyapunov equationdiscrete, 61for composite body inertias

SKO model, 168flexible-link systems, 261geared systems, 237

serial-chain, 61forward, see forward Lyapunov

equationin estimation theory, 112

Lyapunov recursion, 60

Mmass matrix, 58, 67

after aggregation, 291Christoffel symbols, 68, 381computation of the inverse, 198computation using inverse

dynamics, 90constant, 380, 382decomposition, 63, 168, 237, 261decomposition of the inverse, 198determinant, 180diagonal, 380, 382, 393Fixman potential, 376for SKO models, 160for flexible-link systems, 257for geared systems, 236for serial-chain rigid-link system,

82generalized momentum, 67identity matrix, 380, 382Innovations operator factorization,

see Innovations operatorfactorization

invariance to aggregation, 292inverse, v, 198

operator decomposition, 198kinetic energy, 58metric tensor, 381Newton-Euler operator

factorization, seeNewton-Euler operatorfactorization

operator expression, 58operator inverse, 121partitioned, 278

for under-actuated systems, 316recursive computation of, 64, 169,

238, 261Riemannian symbols, 73, 381

Index 503

sensitivity, 365sparsity structure, 282time derivative, 360trace, 65

mass matrix inversealgoritm for computing, 200operator factorization

after constraint embedding, 308for SKO models, 179for flexible-link systems, 263for geared systems, 240for rigid-link serial-chains, 121for under-actuated systems, 319

matrixblock partitioned, 398inverse identities, 398norm, 397Schur complement, 398

metric tensor, 381micro/macro manipulators, 196mixed dynamics, 322modal

integrals, 252joint map matrix, 253mass matrix, 252model for flexible systems,

249–255spatial acceleration, 253spatial displacement, 250spatial displacement influence

vector, 250spatial force, 254spatial velocity, 251, 253stiffness matrix, 254

modal matrix, 250mode shape, 251modes

assumed, 250cantilever, 250free-free, 250

molecular dynamics, 376moments of inertia, 18momentum

generalized, 67spatial, 21

move force, 214move/squeeze

decomposition, 214projection matrix, 214

multibody digraph, 150multibody system

BWA matrix, 150–159SKO operator, 159SKO-forest, 151SKO model, 160SPO operator, 159articulated body model, 97closed-chain, see closed-chain

dynamics, 209, 297composite body model, 97constrained, see closed-chain

dynamics, 209, 297cut-edge, 151digraph, 150

edge, 150node, 150standard, 150

free-flying, 331geared hinge, 229model

articulated body, 97composite body, 97terminal body, 97

serial-chain, 35spanning tree, 151standard digraph, 150terminal body model, 97topological classification, 150tree-topology, see tree-topology

system, 150under-actuated, 313

multiply-connecteddigraph, 137, 209path-induced sub-graph, 274

NNCP, 226Newton–Euler dynamics, 82

equivalence with Lagrangiandynamics, 367

504 Index

Newton–Euler inverse dynamics, 88Newton–Euler operator factorization

determinant, 180diagonalizing transformation, 385flexible-link systems, 257for SKO models, 156for SKO models, 160geared link systems, 236of the mass matrix, 58sensitivity, 375serial-chain systems, 58, 82, 115time derivative, 375tree-topology systems, 291, 302

Newtonian frame, 30, 82nilpotent matrix, 400φ spatial operator, 49BWA matrix, 1431-resolvent, 400tree adjacency matrix, 140

node, 54adjacent, 135ancestor, 135child, 135descendant, 135multibody, 150parent, 135related, 135unrelated, 135weight dimension, see weight

dimensionnode-contraction, 283Noether’s theorem, 34, 329non-Euclidean geometry, 381non-holonomic constraint, 210non-integrable velocity coordinates,

23nonlinear complementarity

problem, 226norm

of a matrix, 397of a vector, 397

Ooperational space

algorithm for computing Υ, 195

compliance kernel, see operationalspace compliance kernel

compliance matrix, 189, 217computation of Coriolis vector,

203control, 189Coriolis generalized forces, 202dynamics, 189equations of motion, 187extended compliance matrix, see

extended operational spacecompliance matrix, 217

inertia, see operational spaceinertia

operational space compliance kernel,191

algorithm for computing, 195base-invariance, 349for free-flying systems, 196, 349for micro/macro systems, 197singularity, 196, 349

operational space inertiaalgorithm for computing, 194base-invariant, 348base-invariant computation of,

351inverse, 189

operator, 48SKO, see SKO operatorSPO, see SPO operatorarticulated body, 116backward Lyapunov equation, 170compatibility, 162derivatives, 353expressions with Bk �= Ok, 83for serial-chain systems, 47forward Lyapunov equation, 166identities, 117, 176Innovations factorization, see

Innovations operatorfactorization

Jacobian, 55mass matrix expression, 58, 120mass matrix inverse expression,

121

Index 505

Newton-Euler factorization, seeNewton-Euler operatorfactorization

pick-off, see pick-off operatorRiccati equation, 174sensitivity, 361–368spatial inertias, 58spatial kernel, see SKO operatorspatial propagation, see SPO

operatortime derivatives, 356–361

operator decompositionofΩ, 191

for serial-chains, 197of M−1, 198

for serial-chains, 199of φMφ∗, 61, 121of φMψ∗, 121of AXB

∗, 166of A

∗XB, 170for serial-chains, 172

of inter-link spatial forces f, 128of the mass matrix M, 63, 168

optimalestimation theory, 111filter, 112, 338smoother, 113, 338smoothing, 112

ordinary differential equation, 70,213, 222, 339

outboard link, 35

Pparallel-axis theorem

for rotational inertias, 19for spatial inertias, 20

parent node, 135parent nodes of a sub-graph, 284parent sub-graph, 274partial velocities, 161partitioning

induced by sub-graphs, 274of a mass matrix, 278of an SKO operator, 276

of an SKO model, 277of an SPO operator, 277of free-flying system, 343of multibody systems, 277using serial-chain segments, 282

passivedegree of freedom, 314arm, 315hinge, 314

path-induced sub-graph, 273, 343aggregation condition, 285induced partitions, 274multiply-connected, 274partition of SKO and SPO

operators, 275with directed cycle, 274

permutationre-indexing of nodes, 140transformation of a BWA matrix,

148Pfaffian form, 210pick-off operator

constraint force, 131external forces, 85gravitational acceleration, 87, 128,

202nodes, 55operational space, 188

polytree digraph, 137positive semi-definiteness

of articulated body inertia, 104of rotational inertias, 19of spatial inertias, 20of the mass matrix, 58

prescribed motion dynamics, 324prismatic hinge, 40

Coriolis acceleration, 79projected dynamics, 212, 221–222

equivalence to augmenteddynamics, 222

forward dynamics, 223projection matrix, 104

articulated body, 102operational space, 199

506 Index

Qquasi-velocities, 23, 382

angular velocity as, 71diagonalizing velocity

coordinates, 382Lagrangian equations of motion,

70quaternions, 41, 406–411E+ matrix, 408E− matrix, 408acceleration, 411attitude representation, 406based transformation of vectors,

410composition of, 409identity element, 409rate from angular velocity, 410rates to angular velocity, 411

Rrate-feedback control, 392reaction-mode control, 329recursion

backward Lyapunov, 171base-to-tip, 52base-to-tips, 163forward Lyapunov, 167gather, 162Lyapunov, 60Riccati equation, 174scatter, 163tip-to-base, 51tips-to-base, 162

regular free-flying system model,332

related nodes, 135Ω simplification, 197SPO operator sparsity, 146, 278backward Lyapunov equation, 174elements in AXB

∗, 167elements in A

∗XB, 172in a serial-chain digraph, 137mass matrix sparsity, 169

relative coordinates, 38residual spatial forces vector, 107

reversinga serial-chain digraph, 141a tree digraph, 140an SKO operator, 345

rheonomic constraint, 210Riccati equation, 105

discrete-time systems, 113for SPO operators, 174operator form, 117

Riemannian symbols of the firstkind, 73, 381

rigid bodycenter of mass, 18conservation of

kinetic energy, 30spatial momentum, 33

dynamics, 25rotational, 71

equations of motion, 25rotational, 71

first moment of inertia, 18gyroscopic force, 26, 30kinetic energy, 18mass, 18moments of inertia, 18second moment of inertia, 18spatial inertia, 18spatial momentum, 21transformation, see rigid body

transformationrigid body dynamics, 17

about arbitrary point, 29–31about center of mass, 25–27about inertially fixed velocity

reference point, 31–33based on spatial momentum, 30using body frame spatial

velocities, 29–31using inertial spatial velocities,

27–29rigid body transformation

matrix, 11–12of ˜V(x), 13of spatial forces, 15of spatial inertias, 20

Index 507

of spatial momentum, 22of spatial velocities, 12operator, 43properties, 22time derivative, 13

Rodrigues vector attituderepresentation, 412

root node, 135rooted digraph, 136

for multibody systems, 150rotary pin hinge, 40

Coriolis acceleration, 79rotation matrix, 4

characteristic polynomial, 405direction-cosine matrix, 403eigen-values, 406eigen-vectors, 404, 405exponential form, 404from angle/axis parameters, 405from Euler angles, 404from quaternions, 406time-derivative, 5, 403trace, 405

rotation of vectors, 412rotational inertia, 18

Sscatter recursion, 163Schur complement, 398scleronomic constraint, 210semi-group

property of φ(i, j) , 50property of ψ(i, j), 117property of A(i, j), 146

sensitivityof D, 374of D−1, 374of G, 374of P, 374of P+, 374of τ, 374of Eψ, 374of H, 364of H(k), 363of φ, 364

of φ(℘(k),k), 363of φMφ∗, 365of M, 364ofM(k), 363of τ, 374of articulated body quantities,

373–377of Innovations factors, 375of Newton–Euler factors, 375of spatial operators, 361–368of the kinetic energy, 366of the mass matrix M, 365of vector quantities, 353

separating contact, 225serial-chain digraph, 137

adjacency matrix properties, 140reversal, 141

serial-chain equations of motion, 81inclusion of external forces, 85inclusion of gravitational forces,

87operator form of, 82using inertially fixed reference

point, 93serial-chain system

Coriolis acceleration, 77, 80, 434equations of motion, see

serial-chain equations ofmotion

inverse dynamics, 88–93kinematics, 35kinetic energy, 57model, 35–42reversal, 345rigid multibody system, 35simple hinge Coriolis

acceleration, 79spatial operators, 47velocity recursion, 46

similarity transformation, 83, 96similarity-shift transformation, 147,

149simple tree digraph, see treesimply-connected digraph, 137skew-symmetric matrix

508 Index

associated with cross-products, 6characteristic polynomial, 405property of (M∗

D−MD), 68SKO formulation

issues for non-tree systems, 185procedure, 182SKO model, 160

SKO model, 159–185AB forward dynamics, 182aggregation condition, 288backward Lyapunov equation, 170definition, 160development procedure, 183existence, 161for aggregated tree, 290for closed-chain systems, 302for flexible systems, 255–257for geared systems, 234for passive system, 317forward dynamics, 180forward Lyapunov equation, 166generalizations, 161identities, 176Innovations operator factorization,

179inverse dynamics, 166mass matrix, 168non-tree generalization, 185partitioning, 275–278Riccati equation, 174with kth link as base-body, 341with constraint embedding, 302with new base-body, 347

SKO operatorBWA matrix, 159after aggregation, 288of reversed serial-chains, 345partitioned, 276

SKO-forest, 151spanning tree, 138

multibody, 151sparsity structure

SPO operator, 278mass matrix, 169, 282of serial-chain EA, 145

of serial-chain A, 145of tree EA, 145of tree A, 145of tree SKO, 280of tree SPO, 281

spatial acceleration, 76from innovations accelerations,

125modal, 253

spatial deformation, 246spatial force, 15

inter-link, 75residual, 107rigid body transformation, 15

spatial inertia, 17–21of full system, 62rigid body transformation, 18spatial operator, 58

spatial inertiascomposition of, 21

spatial kernel operator (SKO), seeSKO operator

spatial momentum, 21of full system, 63rigid body transformation, 22

spatial operator, see operatorspatial propagation operator (SPO),

see SPO operatorspatial vector, 8–14

cross product for, 9–10spatial force, see spatial forcespatial momentum, see spatial

momentumspatial velocity, see spatial

velocityspatial velocity, 9

modal, 251recursive algorithm for, 45relative, 39rigid body transformation, 12

spherical hinge, 40, 41SPO operator

1-resolvent matrix, 159after aggregation, 289backward Lyapunov equation, 170

Index 509

forward Lyapunov equation,166

identities, 176partitioned, 277product with vectors, 162Riccati equation, 174sparsity structure, 278

squeeze force, 214stacked vectors, 48standard digraph, 150strictly canonical tree digraph, 137

adjacency matrix, 140structural

mass matrix, 248stiffness matrix, 248

sub-graph, 273path-induced, 273aggregation, 283child nodes, 284child sub-graph, 274induced, 273induced partitions, 274parent nodes, 284parent sub-graph, 274

sub-structured SKO models, 290symmetries of the Lagrangian, 34

Tterminal-body model, 76, 97time derivative

of H∗I(k), 94

of D, 368of D−1, 368of G, 368of P, 368of P+, 368of τ, 368of Eψ, 368of H, 357of H(k), 356of φ, 357of φ(k+ 1,k), 356, 359of M, 357ofM(k), 356of τ, 368

of articulated body quantities,368–373

of homogenous transforms, 38of Innovations factors, 375of Newton–Euler factors, 375of quasi-coordinates, see

quasi-coordinatesof spatial operators, 356–361of the mass matrix M, 360of the rigid body transformation

matrix, 13time-derivative

of a 3-vector, 6of a rotation matrix, 5of a spatial vector, 10

tip-body, 35tip-to-base recursion, 51tips-to-base gather recursion, 162torque minimization, 215trace

of a rotation matrix, 405of the mass matrix, 65

transformhomogeneous, 4rigid body, see rigid body

transformtransformation

change of base-body, 341diagonalizing, 380of a BWA matrix, 147

tree digraph, 137adjacency matrix properties, 140canonical, 137reversal, 140strictly canonical, 137

tree-topology system, 150Eφ BWA matrix, 155equations of motion, 155

Uunder-actuated system, 313

articulated body inertia, 317decomposition, 315disturbance Jacobian, 325equations of motion, 316

510 Index

flexible-link systems, 261forward dynamics, 322free-flying system, 329–330generalized Jacobian, 325Innovations operator factorization,

319inverse dynamics, 322mixed dynamics, 322modeling, 314–320relation to prescribed motion

systems, 322undirected graph, 135unilateral constraint

complementarity approach, 224distance function, 225forward dynamics, 227gap function, 225penalty method, 224

unit quaternions, see quaternions, 41universal joint, 41unrelated nodes, 135

SPO operator sparsity, 146, 278elements in AXB

∗, 167elements in A

∗XB, 172in tree digraphs, 137mass matrix sparsity, 169

Vvector

derivative, 5–8

norm, 397rotation of, 412

velocitydegrees of freedom, 39diagonalization condition,

382diagonalizing coordinate

transformations, 382–392generalized, see generalized

velocity coordinateshinge coordinates, 39reference frame, 31

velocity recursionfor aggregated body, 294for serial-chains, 44for tree systems, 154with Bk �= Ok, 46with kth link as base-body,

342

Wweight dimension, 141

determination, 184for rigid-link systems, 153

weight matrices, 141determination, 184for dual model, 333for rigid-link systems, 153general properties, 161

Robot and Multibody Dynamics: Analysis and Algorithms

Documents

Transcript of Robot and Multibody Dynamics: Analysis and Algorithms