Rigid Body Dynamics Algorithms

276 Pages·2007·2.69 MB·English

by  Roy Featherstone

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 e-ISBN 978-0-387-74315-8

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
e(cid:14)cient algorithms for calculating rigid-body dynamics, and to explain them
in enough detail that the reader can understand how they work, and how to
adaptthem(orcreatenewalgorithms)tosuitthereader’sneeds. Thecollection
includesthefollowingwell-knownalgorithms: therecursiveNewton-Euleralgo-
rithm,thecomposite-rigid-bodyalgorithmandthearticulated-bodyalgorithm.
It also includes algorithms for kinematic loops and (cid:13)oating bases. Each algo-
rithmisderivedfrom(cid:12)rstprinciples,andispresentedbothasasetofequations
andasapseudocodeprogram,thelatterbeingdesignedforeasytranslationinto
any suitable programming language.
This book also explains some of the mathematical techniques used to for-
mulatethe equationsofmotionforarigid-bodysystem. Inparticular,itshows
howtoexpressdynamicsusingsix-dimensional(6D)vectors,anditexplainsthe
recursiveformulationsthatarethebasisofthemoste(cid:14)cientalgorithms. 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 largelythe result of using3D vectors,and it can be remedied byusinga
6Dvectornotationinstead. Thisbookusesanotationbasedonspatial vectors,
inwhichthelinearandangularaspectsofrigid-bodymotionarecombinedinto
a uni(cid:12)ed set of quantities and equations. The result is typically a four- to six-
foldreductioninthevolumeofalgebra. Thebene(cid:12)tisalsofeltinthecomputer
code: shorter, clearer programs that are easier to read, write and debug, but
are still just as e(cid:14)cient 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
tohavesomepriorknowledgeofrigid-bodydynamics,suchasmightbeobtained
fromanintroductorycourseondynamics,orfromreadingthe(cid:12)rstfewchapters
ofanintroductorytext. However,nopriorknowledgeof6Dvectorsisrequired,
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 (cid:13)oating-base
systems; planar vectors (the planar equivalent of spatial vectors); numerical
errorsand model sensitivity; and guidance on how to implement spatial-vector
arithmetic.
Contents
Preface v
1 Introduction 1
1.1 Dynamics Algorithms . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Spatial Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Units and Notation . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Readers’ Guide . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.5 Further Reading . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Spatial Vector Algebra 7
2.1 Mathematical Preliminaries . . . . . . . . . . . . . . . . . . . . . 7
2.2 Spatial Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Spatial Force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4 Plu(cid:127)cker Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.5 Line Vectors and Free Vectors . . . . . . . . . . . . . . . . . . . . 16
2.6 Scalar Product . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.7 Using Spatial Vectors . . . . . . . . . . . . . . . . . . . . . . . . 18
2.8 Coordinate Transforms . . . . . . . . . . . . . . . . . . . . . . . . 20
2.9 Spatial Cross Products . . . . . . . . . . . . . . . . . . . . . . . . 23
2.10 Di(cid:11)erentiation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.11 Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.12 Momentum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.13 Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.14 Equation of Motion. . . . . . . . . . . . . . . . . . . . . . . . . . 35
2.15 Inverse Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
2.16 Planar Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
2.17 Further Reading . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
3 Dynamics of Rigid Body Systems 39
3.1 Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.2 Constructing Equations of Motion . . . . . . . . . . . . . . . . . 42
3.3 Vector Subspaces . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.4 Classi(cid:12)cation of Constraints . . . . . . . . . . . . . . . . . . . . . 50
vii
viii CONTENTS
3.5 Joint Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
3.6 Dynamics of a Constrained Rigid Body . . . . . . . . . . . . . . 57
3.7 Dynamics of a Multibody System . . . . . . . . . . . . . . . . . . 60
4 Modelling Rigid Body Systems 65
4.1 Connectivity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
4.2 Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.3 Denavit-Hartenberg Parameters . . . . . . . . . . . . . . . . . . . 75
4.4 Joint Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.5 Spherical Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
4.6 A Complete System Model . . . . . . . . . . . . . . . . . . . . . 87
5 Inverse Dynamics 89
5.1 Algorithm Complexity . . . . . . . . . . . . . . . . . . . . . . . . 89
5.2 Recurrence Relations . . . . . . . . . . . . . . . . . . . . . . . . . 90
5.3 The Recursive Newton-Euler Algorithm . . . . . . . . . . . . . . 92
5.4 The Original Version . . . . . . . . . . . . . . . . . . . . . . . . . 97
5.5 Additional Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
6 Forward Dynamics | Inertia Matrix Methods 101
6.1 The Joint-Space Inertia Matrix . . . . . . . . . . . . . . . . . . . 102
6.2 The Composite-Rigid-Body Algorithm . . . . . . . . . . . . . . . 104
6.3 A Physical Interpretation . . . . . . . . . . . . . . . . . . . . . . 108
6.4 Branch-Induced Sparsity . . . . . . . . . . . . . . . . . . . . . . . 110
6.5 Sparse Factorization Algorithms . . . . . . . . . . . . . . . . . . 112
6.6 Additional Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
7 Forward Dynamics | Propagation Methods 119
7.1 Articulated-Body Inertia . . . . . . . . . . . . . . . . . . . . . . . 119
7.2 Calculating Articulated-Body Inertias . . . . . . . . . . . . . . . 123
7.3 The Articulated-Body Algorithm . . . . . . . . . . . . . . . . . . 128
7.4 Alternative Assembly Formulae . . . . . . . . . . . . . . . . . . . 131
7.5 Multiple Handles . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
8 Closed Loop Systems 141
8.1 Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . 141
8.2 Loop Constraint Equations . . . . . . . . . . . . . . . . . . . . . 143
8.3 Constraint Stabilization . . . . . . . . . . . . . . . . . . . . . . . 145
8.4 Loop Joint Forces. . . . . . . . . . . . . . . . . . . . . . . . . . . 148
8.5 Solving the Equations of Motion . . . . . . . . . . . . . . . . . . 149
8.6 Algorithm for C (cid:28)a . . . . . . . . . . . . . . . . . . . . . . . . 152
(cid:0)
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.11 Loop Closure Functions . . . . . . . . . . . . . . . . . . . . . . . 161
8.12 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
8.13 Sparse Matrix Method . . . . . . . . . . . . . . . . . . . . . . . . 166
9 Hybrid Dynamics and Other Topics 171
9.1 Hybrid Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
9.2 Articulated-Body Hybrid Dynamics . . . . . . . . . . . . . . . . 176
9.3 Floating Bases . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
9.4 Floating-Base Forward Dynamics . . . . . . . . . . . . . . . . . . 181
9.5 Floating-Base Inverse Dynamics. . . . . . . . . . . . . . . . . . . 183
9.6 Gears . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
9.7 Dynamic Equivalence. . . . . . . . . . . . . . . . . . . . . . . . . 189
10 Accuracy and E(cid:14)ciency 195
10.1 Sources of Error . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
10.2 The Sensitivity Problem . . . . . . . . . . . . . . . . . . . . . . . 199
10.3 E(cid:14)ciency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
10.4 Symbolic Simpli(cid:12)cation . . . . . . . . . . . . . . . . . . . . . . . 209
11 Contact and Impact 213
11.1 Single Point Contact . . . . . . . . . . . . . . . . . . . . . . . . . 213
11.2 Multiple Point Contacts . . . . . . . . . . . . . . . . . . . . . . . 216
11.3 A Rigid-Body System with Contacts . . . . . . . . . . . . . . . . 219
11.4 Inequality Constraints . . . . . . . . . . . . . . . . . . . . . . . . 222
11.5 Solving Contact Equations. . . . . . . . . . . . . . . . . . . . . . 224
11.6 Contact Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . 227
11.7 Impulsive Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 230
11.8 Soft Contact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235
11.9 Further Reading . . . . . . . . . . . . . . . . . . . . . . . . . . . 239
A Spatial Vector Arithmetic 241
A.1 Simple Planar Arithmetic . . . . . . . . . . . . . . . . . . . . . . 241
A.2 Simple Spatial Arithmetic . . . . . . . . . . . . . . . . . . . . . . 243
A.3 Compact Representations . . . . . . . . . . . . . . . . . . . . . . 245
A.4 Axial Screw Transforms . . . . . . . . . . . . . . . . . . . . . . . 249
A.5 Some E(cid:14)ciency Tricks . . . . . . . . . . . . . . . . . . . . . . . . 252
Bibliography 257
Symbols 265
Index 267
Chapter 1
Introduction
Rigid-body dynamics is an old subject that has been rejuvenated and trans-
formedbythecomputer. Today,wecan(cid:12)nddynamicscalculationsincomputer
games, in animation and virtual-realitysoftware, in simulators, in motion con-
trolsystems,andinavarietyofengineeringdesignandanalysistools. Inevery
case, a computer is calculating the forces, accelerations, and so on, associated
with the motion of a rigid-body approximation of a physical system.
The main purpose of this book is to present a collection of e(cid:14)cient algo-
rithms for performing various dynamics calculations on a computer. Each al-
gorithm is described in detail, so that the reader can understand how it works
and why it is e(cid:14)cient; and basic concepts are explained, such as recursive for-
mulation, branch-induced sparsity and articulated-body inertia.
Rigid-body dynamics is usually expressed using 3D vectors. However, the
subjectofthisbookisdynamicsalgorithms,andthissubjectisbetterexpressed
using 6D vectors. We therefore adopt a 6D notation based on spatial vectors,
which is explained in Chapter 2. Compared with 3D vectors, spatial notation
greatly reduces the volume of algebra, simpli(cid:12)es the tasks of describing and
explainingadynamicsalgorithm,andsimpli(cid:12)estheprocessofimplementingan
algorithm on a computer.
This chapter provides a short introduction to the subject matter of this
book. It saysa little about dynamics algorithms,a little about spatial vectors,
and it explains how the book is organized.
1.1 Dynamics Algorithms
Thedynamicsofarigid-bodysystemisdescribeditsequationofmotion,which
speci(cid:12)es the relationship between the forces acting on the system and the ac-
celerations they produce. A dynamics algorithm is a procedure for calculating
the numeric values of quantities that are relevant to the dynamics. We will be
concerned mainly with algorithms for two particular calculations: