Rigid Body Dynamics Algorithms

Roy Featherstone

Rigid Body Dynamics

Algorithms

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-

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,

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.

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.1 Dynamics Algorithms . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Spatial Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 Units and Notation . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.4 Pl¨ucker 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 Differentiation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.11 Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

2.12 Momentum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

2.13 Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.2 Constructing Equations of Motion . . . . . . . . . . . . . . . . . 42

3.3 Vector Subspaces . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3.4 Classification 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

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

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 − τ

a

. . . . . . . . . . . . . . . . . . . . . . . . 152

8.7 Algorithm for K and k . . . . . . . . . . . . . . . . . . . . . . . 154