Rigid Body Dynamics Algorithms Roy Featherstone Rigid Body Dynamics Algorithms Roy Featherstone The Austrailian National University Canberra, ACT Austrailia Library of Congress Control Number: 2007936980 ISBN 978-0-387-74314-1 ISBN 978-1-4899-7560-7 (eBook) Printed on acid-free paper. @ 2008 Springer Science+Business Media, LLC All rights reserved. This work may not be translated or copied in whole or in part without the written permission 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 in connection 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 are not identified as such, is not to be taken as an expression of opinion as to whether or not they are subject to proprietary rights. 9 8 7 6 5 4 3 2 1 springer.com Preface The purpose of this book is to present a substantial collection of the most efficient algorithms for calculating rigid-body dynamics, and to explain them in enough detail that the reader can understand how they work, and how to adapt them (or create new algorithms) to suit the reader’s needs. The collection includes the following well-known algorithms: the recursive Newton-Euler algo- rithm, the composite-rigid-body algorithm and the articulated-body algorithm. It also includes algorithms for kinematic loops and floating bases. Each algo- rithm is derived from first principles, and is presented both as a set of equations and as a pseudocode program, the latter being designed for easy translation into any suitable programming language. This book also explains some of the mathematical techniques used to for- mulate the equations of motion for a rigid-body system. In particular, it shows how to express dynamics using six-dimensional (6D) vectors, and it explains the recursive formulations that are the basis of the most efficient algorithms. Other topics include: how to construct a computer model of a rigid-body system; ex- ploiting sparsity in the inertia matrix; the concept of articulated-body inertia; the sources of rounding error in dynamics calculations; and the dynamics of physical contact and impact between rigid bodies. Rigid-body dynamics has a tendency to become a sea of algebra. However, this is largely the result of using 3D vectors, and it can be remedied by using a 6D vector notation instead. This book uses a notation based on spatial vectors, in which the linear and angular aspects of rigid-body motion are combined into a unified set of quantities and equations. The result is typically a four- to six- fold reduction in the volume of algebra. The benefit is also felt in the computer code: shorter, clearer programs that are easier to read, write and debug, but are still just as efficient as code using standard 3D vectors. This book is intended to be accessible to a wide audience, ranging from senior undergraduates to researchers and professionals. Readers are assumed to have some prior knowledge of rigid-body dynamics, such as might be obtained from an introductory course on dynamics, or from reading the first few chapters of an introductory text. However, no prior knowledge of 6D vectors is required, as this topic is explained from the beginning in Chapter 2. This book does also contain some advanced material, such as might be of interest to dynamics vi PREFACE experts and scholars of 6D vectors. No software is distributed with this book, but readers can obtain source code for most of the algorithms described here from the author’s web site. This text was originally intended to be a second edition of a book entitled Robot Dynamics Algorithms, which was published back in 1987; but it quickly became clear that there was enough new material to justify the writing of a whole new book. Compared with its predecessor, the most notable new mate- rials to be found here are: explicit pseudocode descriptions of the algorithms; a chapter on how to model rigid-body systems; algorithms to exploit branch- induced sparsity; an enlarged treatment of kinematic loops and floating-base systems; planar vectors (the planar equivalent of spatial vectors); numerical errors and model sensitivity; and guidance on how to implement spatial-vector arithmetic. Contents Preface v 1 Introduction 1 1.1DynamicsAlgorithms........................ 1 1.2SpatialVectors............................ 3 1.3UnitsandNotation.......................... 4 1.4Readers’Guide............................ 5 1.5FurtherReading........................... 6 2 Spatial Vector Algebra 7 2.1MathematicalPreliminaries..................... 7 2.2SpatialVelocity............................ 10 2.3SpatialForce............................. 13 2.4 Pl¨uckerNotation........................... 15 2.5LineVectorsandFreeVectors.................... 16 2.6ScalarProduct............................ 17 2.7UsingSpatialVectors........................ 18 2.8CoordinateTransforms........................ 20 2.9SpatialCrossProducts........................ 23 2.10Differentiation............................ 25 2.11Acceleration.............................. 28 2.12Momentum.............................. 31 2.13Inertia................................. 32 2.14EquationofMotion.......................... 35 2.15InverseInertia............................ 36 2.16PlanarVectors............................ 37 2.17FurtherReading........................... 38 3 Dynamics of Rigid Body Systems 39 3.1EquationsofMotion......................... 40 3.2ConstructingEquationsofMotion................. 42 3.3VectorSubspaces........................... 46 3.4ClassificationofConstraints..................... 50 vii viii CONTENTS 3.5JointConstraints........................... 53 3.6 Dynamics of a Constrained Rigid Body . 57 3.7DynamicsofaMultibodySystem.................. 60 4 Modelling Rigid Body Systems 65 4.1Connectivity............................. 66 4.2Geometry............................... 73 4.3Denavit-HartenbergParameters................... 75 4.4JointModels............................. 78 4.5SphericalMotion........................... 84 4.6ACompleteSystemModel..................... 87 5 Inverse Dynamics 89 5.1AlgorithmComplexity........................ 89 5.2RecurrenceRelations......................... 90 5.3 The Recursive Newton-Euler Algorithm . 92 5.4TheOriginalVersion......................... 97 5.5AdditionalNotes........................... 99 6 Forward Dynamics — Inertia Matrix Methods 101 6.1TheJoint-SpaceInertiaMatrix...................102 6.2 The Composite-Rigid-Body Algorithm . 104 6.3APhysicalInterpretation......................108 6.4Branch-InducedSparsity.......................110 6.5 Sparse Factorization Algorithms . 112 6.6AdditionalNotes...........................117 7 Forward Dynamics — Propagation Methods 119 7.1Articulated-BodyInertia.......................119 7.2 Calculating Articulated-Body Inertias . 123 7.3TheArticulated-BodyAlgorithm..................128 7.4AlternativeAssemblyFormulae...................131 7.5MultipleHandles...........................136 8 Closed Loop Systems 141 8.1EquationsofMotion.........................141 8.2LoopConstraintEquations.....................143 8.3 Constraint Stabilization . 145 8.4LoopJointForces...........................148 8.5SolvingtheEquationsofMotion..................149 8.6 Algorithm for C − τ a ........................152 8.7 Algorithm for K and k .......................154 8.8 Algorithm for G and g ........................156 8.9 Exploiting Sparsity in K and G ..................158 8.10 Some Properties of Closed-Loop Systems . 159 CONTENTS ix 8.11LoopClosureFunctions.......................161 8.12InverseDynamics...........................164 8.13SparseMatrixMethod........................166 9 Hybrid Dynamics and Other Topics 171 9.1HybridDynamics...........................171 9.2 Articulated-Body Hybrid Dynamics . 176 9.3FloatingBases............................179 9.4Floating-BaseForwardDynamics..................181 9.5Floating-BaseInverseDynamics...................183 9.6Gears.................................186 9.7DynamicEquivalence.........................189 10 Accuracy and Efficiency 195 10.1SourcesofError...........................196 10.2TheSensitivityProblem.......................199 10.3Efficiency...............................201 10.4SymbolicSimplification.......................209 11 Contact and Impact 213 11.1SinglePointContact.........................213 11.2MultiplePointContacts.......................216 11.3 A Rigid-Body System with Contacts . 219 11.4InequalityConstraints........................222 11.5SolvingContactEquations......................224 11.6ContactGeometry..........................227 11.7ImpulsiveDynamics.........................230 11.8SoftContact.............................235 11.9FurtherReading...........................239 A Spatial Vector Arithmetic 241 A.1SimplePlanarArithmetic......................241 A.2SimpleSpatialArithmetic......................243 A.3CompactRepresentations......................245 A.4AxialScrewTransforms.......................249 A.5SomeEfficiencyTricks........................252 Bibliography 257 Symbols 265 Index 267 Chapter 1 Introduction Rigid-body dynamics is an old subject that has been rejuvenated and trans- formed by the computer. Today, we can find dynamics calculations in computer games, in animation and virtual-reality software,