189
UNIVERSITY OF CALIFORNIA, SAN DIEGO
Multibody Dynamical Algorithms,
Numerical Optimal Control,
with Detailed Studies in the
Control of Jet Engine Compressors
and Biped Walking
A dissertation submitted in partial satisfaction of the
requirements for the degree Doctor of Philosophy
in Electrical Engineering (Intelligent Systems,
Robotics, and Control)
by
Michael William Hardt
Committee in charge:
Professor Kenneth Kreutz-Delgado, Chairperson
Professor J. William Helton
Professor Mohan Trivedi
Professor David D. SworderProfessor Robert E. Skelton
1999
Copyright
Michael William Hardt, 1999
All rights reserved.
The dissertation of Michael William Hardt is approved,
and it is acceptable in quality and form for publication
on micro�lm:
Chair
University of California, San Diego
1999
iii
TABLE OF CONTENTS
Signature Page . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii
Table of Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x
Vita and Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
I Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1A. Articulated, Multibody Systems . . . . . . . . . . . . . . . . . . . . 2
1. Some Previous Work on Dynamics Algorithms . . . . . . . . . . 4
B. Nonlinear H1 and Numerical Optimal Control . . . . . . . . . . . . 5C. Case Study: Jet Engine Compressors . . . . . . . . . . . . . . . . . 6
1. Some Previous Work on Jet Engine Compressor Control . . . . . 7D. Case Study: Minimum Energy Biped Walking . . . . . . . . . . . . 7
1. Some Previous Work on Biped Locomotion . . . . . . . . . . . . 8
E. Outline of Dissertation . . . . . . . . . . . . . . . . . . . . . . . . . 9
II Recursive, Symbolic Robot Dynamics . . . . . . . . . . . . . . . . . . . 10A. A Single Rigid Body . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1. Dynamics in Inertial Space . . . . . . . . . . . . . . . . . . . . . 112. Dynamics in Body Space . . . . . . . . . . . . . . . . . . . . . . 163. Dynamics in the Spatial Representation . . . . . . . . . . . . . . 17
B. Multiple Rigid Bodies in Inertial Space . . . . . . . . . . . . . . . . 20
C. Multiple Rigid Bodies in Body Space . . . . . . . . . . . . . . . . . 26D. Multiple Rigid Bodies with the Spatial Representation . . . . . . . . 30
E. Comparison of Dynamical Representations . . . . . . . . . . . . . . 31
F. Dynamics with Gravity . . . . . . . . . . . . . . . . . . . . . . . . . 34G. Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
H. Tree-Structured Dynamics and Spatial Recursions . . . . . . . . . . 40
III Contact and Collision Robot Dynamics . . . . . . . . . . . . . . . . . . 45
A. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45B. Contact Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
C. Collision Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 51D. Calculation of ��1 in the Contact and Collision Algorithms . . . . . 53
iv
E. Introduction of External Forces . . . . . . . . . . . . . . . . . . . . . 57
F. Reduced Dynamics Algorithm . . . . . . . . . . . . . . . . . . . . . 57
G. Closed-Chain Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 61
IV Derivatives of Spatial Dynamic Operators and Applications . . . . . . . 62
A. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
B. Derivatives of Spatial Operators . . . . . . . . . . . . . . . . . . . . 63
C. A Lagrangian Derivation of the Dynamics . . . . . . . . . . . . . . . 67
D. Derivation of Coriolis Matrix for Skew-Symmetry
Property . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
E. Time Derivatives of Energy . . . . . . . . . . . . . . . . . . . . . . . 75
F. Spatially Recursive Linearized Dynamics . . . . . . . . . . . . . . . 76
V Nonlinear H2 and H1 Control and Numerical Solution Techniques . . . 84A. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
B. Nonlinear H2 and H1 Problems . . . . . . . . . . . . . . . . . . . . . 861. Nonlinear H2Control and the HJB PDE . . . . . . . . . . . . . . 862. Bicharacteristic ODEs . . . . . . . . . . . . . . . . . . . . . . . . 873. Optimal State Feedback Law . . . . . . . . . . . . . . . . . . . . 884. Nonlinear H1 Control . . . . . . . . . . . . . . . . . . . . . . . . 88
C. Proposed Numerical Solution of Nonlinear H2 and H1
Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 891. Direct Minimization . . . . . . . . . . . . . . . . . . . . . . . . . 892. Multiple Shooting . . . . . . . . . . . . . . . . . . . . . . . . . . 903. Shooting Out . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 914. Combined Method . . . . . . . . . . . . . . . . . . . . . . . . . . 92
5. Homotopy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92D. Other Numerical Methods . . . . . . . . . . . . . . . . . . . . . . . . 93
1. Finite di�erences . . . . . . . . . . . . . . . . . . . . . . . . . . . 932. "Small" basis of functions . . . . . . . . . . . . . . . . . . . . . . 933. Partly Analytical Methods . . . . . . . . . . . . . . . . . . . . . 94
4. Characteristics and Direct Minimization . . . . . . . . . . . . . . 95
5. Miscellaneous . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95E. Illustration: Pendulum . . . . . . . . . . . . . . . . . . . . . . . . . 96
1. Nonlinear H2 Solution and Comparison of Numerical Methods . . 97
2. Multiple Sheets of the Value Function . . . . . . . . . . . . . . . 98
F. Recursive Calculation of Robot Adjoint Equations . . . . . . . . . . 102G. Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
VI Case Study: Optimal Control of Jet Engine Compressors . . . . . . . . 106A. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
B. Jet Engine Stall and Surge Control . . . . . . . . . . . . . . . . . . . 107
1. Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . 109
v
2. Choice of Performance Index . . . . . . . . . . . . . . . . . . . . 110
3. Rate Saturation Constraints . . . . . . . . . . . . . . . . . . . . 110
4. Choice of Equilibrium . . . . . . . . . . . . . . . . . . . . . . . . 112
5. Calculation of Value Function . . . . . . . . . . . . . . . . . . . . 114
6. State Feedback Control . . . . . . . . . . . . . . . . . . . . . . . 116
7. Evaluation of Results . . . . . . . . . . . . . . . . . . . . . . . . 117
8. Timing and Scaling to Higher Dimensions . . . . . . . . . . . . . 119
C. Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
VII Case Study: Minimum Energy Biped Walking . . . . . . . . . . . . . . 122
A. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
B. Human Walking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
C. Biped Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
D. Biped Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1291. Dynamics with Contact and Collision . . . . . . . . . . . . . . . 1302. Reduced Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 132
E. Minimum Energy Performance . . . . . . . . . . . . . . . . . . . . . 135F. Numerical Optimal Control . . . . . . . . . . . . . . . . . . . . . . . 137
1. Box Constraints and Polar Position Coordinates . . . . . . . . . 1382. Inequality Constraints and Boundary Conditions . . . . . . . . . 141
G. Optimization Trials . . . . . . . . . . . . . . . . . . . . . . . . . . . 1451. Models 1 and 2: Walking without lifto� forces . . . . . . . . . . 1472. Models 3 and 4: Walking with lifto� forces . . . . . . . . . . . . 1503. Varying Step Length, Forward Velocity, and Final Time . . . . . 152
4. Optimal Forward Velocities and Energy Requirements . . . . . . 155
VIIIConclusions and Future Directions . . . . . . . . . . . . . . . . . . . . . 157
A Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160A. De�nitions and Notation . . . . . . . . . . . . . . . . . . . . . . . . 160B. Tilde Identities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
C. Spatial Matrix Identities . . . . . . . . . . . . . . . . . . . . . . . . 163D. Miscellaneous Identities . . . . . . . . . . . . . . . . . . . . . . . . . 166
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
vi
LIST OF FIGURES
2.1 A serial multibody chain . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2 Full Link-Level, Spatial, Dynamical Recursions for Tree-Structured
Systems Including the E�ects of Gravity. . . . . . . . . . . . . . . . 43
3.1 Recursive algorithm for calculating ��1 in the case of branch tip
contact or collision. . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.1 System Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
5.2 Multiple Shooting Method . . . . . . . . . . . . . . . . . . . . . . . 91
5.3 Pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
5.4 Plot of optimal trajectories calculated in the state space to the equi-
librium x = (�; _�) = (�1:5; 0). . . . . . . . . . . . . . . . . . . . . . 995.5 Homotopy in x1(0), (x2(0) = 0) along two sheets of pendulum. The
dotted lines indicate the continuation of a sheet along which thenumerical methods will continue to calculate solutions though they
are no longer optimal. . . . . . . . . . . . . . . . . . . . . . . . . . 1005.6 Equal-cost Boundary on Pendulum . . . . . . . . . . . . . . . . . . 1015.7 Value function of pendulum. . . . . . . . . . . . . . . . . . . . . . . 102
6.1 Jet Engine Compressor . . . . . . . . . . . . . . . . . . . . . . . . . 1086.2 Control vs. time for the H2 optimal trajectory and di�erent per-
formance measures. The solid line is with respect to a quadraticperformance index, and the dashdot and dashed lines are with re-spect to performance indices with cross-terms with weights c5 = 1
and 5 respectively. The circles represent the system initialized atR = 0 with a quadratic performance. . . . . . . . . . . . . . . . . . 111
6.3 vs. time illustrating e�ects of introducing di�erent constraints on_ , the control rate. The solid line is the unconstrained trajectorywhile the dashdot and dashed line represent a constraint of j _ j � 0:2
and j _ j � 0:1 respectively. . . . . . . . . . . . . . . . . . . . . . . . 1126.4 Controlling to di�erent equilibria along compressor characteristic at
a starting initial value of R = 0:12. Trajectories move in a counter-
clockwise direction. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1136.5 A contour plot of a slice of the value function at fR = 0:12g. The �
indicates the equilibrium. . . . . . . . . . . . . . . . . . . . . . . . . 115
6.6 A contour plot of the minimumvalues of attained along closed-loop
system trajectories initialized at di�erent (�;) and at R = 0:12 inthe state space. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
vii
6.7 A contour plot of the maximum absolute di�erence values of � from
the equilibrium value �0 (max j� � �0j) attained along closed-loop
system trajectories initialized at di�erent (�;) and at R = 0:12 in
the state space. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
6.8 A contour plot of the maximum values of R attained along closed-
loop system trajectories initialized at di�erent (�;) and at R =
0:12 in the state space. . . . . . . . . . . . . . . . . . . . . . . . . . 120
7.1 Walking Phases . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
7.2 Beginning of phase 1. An impulse force fimp helps to propel the body
forward. The contact force fc1 is applied to the tip of leg 1. . . . . . 131
7.3 End of phase 2. The contact forces fc1 and fc2 enforce the contact
constraints for legs 1 and 2 during phase 2. . . . . . . . . . . . . . . 131
7.4 Inverse Kinematics Problem for 2-link Leg . . . . . . . . . . . . . . 1347.5 Optimal applied torques for walking without lifto� forces. Included
extra forces in addition to hip, knee torques for optimal speed: Ankle(solid), None (dashed). Included extra forces for 50 m/min speed:Ankle (dashdot), None (dotted) . . . . . . . . . . . . . . . . . . . . 148
7.6 Optimal trajectories of hip, knee for walking without lifto� forces.Included extra forces in addition to hip, knee torques for optimal
speed: Ankle (solid), None (dashed). Included extra forces for 50m/min speed: Ankle (dashdot), None (dotted) . . . . . . . . . . . . 149
7.7 Optimal trajectories of hip position and velocity for walking withoutlifto� forces. Included extra forces in addition to hip, knee torquesfor optimal speed: Ankle (solid), None (dashed). Included extra
forces for 50 m/min speed: Ankle (dashdot), None (dotted) . . . . . 1507.8 Optimal applied torques for walking with lifto� forces. Included
forces in addition to hip, knee torques and impulsive force for optimalspeed: Ankle (solid), None (dashed). Included forces for 50 m/min
speed: Ankle (dashdot), None (dotted) . . . . . . . . . . . . . . . . 151
7.9 Optimal trajectories of hip position and velocity for walking withlifto� forces. Included extra forces in addition to hip, knee torques
for optimal speed: Ankle (solid), None (dashed). Included extra
forces for 50 m/min speed: Ankle (dashdot), None (dotted) . . . . . 1527.10 Required energy as a function of average forward velocity. Ankle ac-
tuation (solid), no ankle forces (dashed), ankle actuation and impul-sive force (dashdot), impulsive force with no ankle actuation (dotted).153
7.11 Step length as a function of average forward velocity. Ankle actua-tion (solid), no ankle forces (dashed), ankle actuation and impulsive
force (dashdot), impulsive force with no ankle actuation (dotted). . 1547.12 Kinetic (dashed), Potential (dotted), and Total (solid) energies cen-
tered at 0 during a walking step. . . . . . . . . . . . . . . . . . . . 155
viii
LIST OF TABLES
2.1 Comparison of Gravity-Free Inverse Dynamics Representations (Cur-
rent Notation). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.2 Comparison of Gravity-Free Inverse Dynamics Representations. (Orig-
inally Presented Notation) . . . . . . . . . . . . . . . . . . . . . . . 33
2.3 Inverse Dynamics with Gravity for Di�erent Representations. . . . . 36
4.1 Time Derivatives of Spatial Operators . . . . . . . . . . . . . . . . . 64
6.1 Comparison of di�erent equilibria. . . . . . . . . . . . . . . . . . . . 114
7.1 Biped Model Physical Data . . . . . . . . . . . . . . . . . . . . . . 127
ix
ACKNOWLEDGEMENTS
The work presented in this thesis would not have been possible if it were
not for the continual support and encouragement of my two advisors Ken Kreutz-
Delgado and Bill Helton. I am deeply grateful to them for that. I would also like
to thank various other researchers for their assistance in my research. Oskar von
Stryk was kind enough to allow me the use of his program upon which many of the
results in this thesis are based. I`d like to thank Michael Breitner for his assistance
on using the programs and other issues in numerical optimal control. I am also
indebted to P. Hiltmann and P. Gill for the use of their programs. Many thanks
to M. Krsti�c, D. Mayne, R.M. Murray, and to P.V. Kokotovi�c, and his students
for their comments and suggestions related to the work on jet engine compressor
control.
The text of Chapters V and VI, in part, are a reprint of the material
as it will appear in IEEE Control Systems Technology under the title \Numerical
Solution of Nonlinear H2 and H1 Control Problems with Application to Jet Engine
Compressors," and with authors, Michael Hardt, J. William Helton, and Kenneth
Kreutz-Delgado. I was the primary researcher and author and the co-authors listed
in this publication directed and supervised the research which forms the basis for
these chapters.
My experience at UCSD would also not have been nearly as enjoyable and
rewarding if it were not for the many discussions and times spent with coworkers
Shane Cotter and Manohar Murthi, as well as my many good friends; in particu-
lar, Ra�aele Parisi, Vassiriki Traore, and Andreas Heinen. Lastly, I would like to
recognize my family for their continual support and patience with me. I am very
lucky to have them.
x
VITA
June 28, 1969 Born, San Diego, California
1991 B.S. in Mathematics/Applied Science
University of California, Los Angeles
Los Angeles, CA
1993 M.A. in Mathematics/Statistics
University of California, Los Angeles
Los Angeles, CA
1999 Doctor of Philosophy
University of California, San Diego
San Diego, CA
PUBLICATIONS
\Using the Algebraic Structure of Articulated Robot Dynamics in Control Design,"36th IEEE Conference on Decision and Control, Vol. 5, pp. 4850{5, December,
1997.
\Numerical Solution of Nonlinear H2 and H1 Control Problems with Applicationto Jet Engine Control," 36th IEEE Conf. on Decision and Control, Vol. 3, pp.2317{22, December, 1997.
\Minimal Energy Control of a Biped Robot with Numerical Methods and a Recur-sive Symbolic Dynamic Model," 37th IEEE Conference on Decision and Control,pp. 413{6, 1998.
\Numerical Solution of Nonlinear H2 and H1 Control Problems with Applicationto Jet Engine Compressors," IEEE Control Systems Technology. (To appear)
\Optimal Biped Walking with a Complete Dynamical Model," 38th IEEE Confer-
ence on Decision and Control, 1999. (To appear)
FIELDS OF STUDY
Major Field: Electrical Engineering
Studies in Mathematical Analysis.
Professors D. Wulbert and R. Getoor
Studies in Numerical Analysis.
Professor P. Gill
xi
Studies in Geometrical Physics.
Professor J. Rabin
Studies in Linear and Nonlinear Control.
Professors K. Kreutz-Delgado, J.W. Helton, and M. Krsti�c
xii
ABSTRACT OF THE DISSERTATION
Multibody Dynamical Algorithms,
Numerical Optimal Control,
with Detailed Studies in the
Control of Jet Engine Compressors
and Biped Walking
by
Michael William Hardt
Doctor of Philosophy in Electrical Engineering
(Intelligent Systems, Robotics, and Control)
University of California, San Diego, 1999
Professor Kenneth Kreutz-Delgado, Chair
Various new methods for the modeling and control of complex mechanical
systems are developed, and new techniques are presented to combat the \curse of
dimensionality" one often encounters with high dimensional systems. This is done
�rst in the context of developing e�cient calculational tools for multibody dynamics
and second for nonlinear H2 and H1 optimal control problems. Their uses and
e�ectiveness are then explored through some important, practical applications.
The �rst focus is on articulated, multibody systems which may collide
and have contact with its surrounding environment. Di�erent lines of research
into recursive, multibody dynamics are brought together along with new, concise
derivations. The development has a great deal of generality allowing for multiple
degrees of freedom joints, complex tree-structured systems, contact and collision
dynamics, and problems requiring derivatives of the dynamics.
The second main focus is on numerical optimal control where the ability
to solve nonlinear H2 and H1 control problems is explored. An extensive listing of
xiii
proven numerical procedures to solve these problems is presented. One particular
proposed approach is discussed at greater length and its strengths and weaknesses
are explored through some illustrations. Numerical experiments are performed
which provide valuable insight to the potential user of these methods as to the
capabilities and limitations of the proposed numerical procedures. An important
case study is then presented which describes the optimal control of jet engine com-
pressors.
The two research avenues converge in the �nal case study on walking biped
robots. With the use of the recursive, symbolic modeling techniques and numerical
optimal control procedures, it is possible to solve the problem of minimum energy
gait generation of biped robots with a complete dynamical model. This problem is
complicated by contact, collision, and other algebraic forms of constraints which re-
quire special attention. Various forms of minimum energy walking are investigated
and analyzed.
xiv
Chapter I
Introduction
The study of complex, interacting dynamical systems has received interest
for hundreds of years. Many mechanical systems composed of interacting subsys-
tems fall into this class and, as the number of the interacting subsystems grows, its
dimension and, correspondingly, its complexity grows. Typically, approximations
of various forms have been made in the past in order to study and control these
systems. Problems of high dimension su�er what is known as the \curse of dimen-
sionality" where the problem complexity grows at such a fast rate with increasing
dimension that even systems with a 5 or 6-dimensional state space may be too
complex to handle.
The overall focus of this work presented here is to investigate and develop
new methods for beating the \curse of dimensionality" of complex, mechanical sys-
tems in the context of modeling and control. The �rst direction taken is to develop
and extend existing models for recursively and symbolically representing articu-
lated, multibody dynamics. Next, we will describe various numerical techniques
for solving nonlinear H2 and H1 control problems and report on solutions of some
di�cult control problems using these methods. The last case study will combine
the research into multibody dynamics with that of numerical optimal control by
exploring minumum energy gait patterns for biped robots.
1
2
I.A Articulated, Multibody Systems
I begin with a detailed study of articulated, multibody systems. Such
systems are perfect examples of collections of relatively simple systems which when
interconnected produce a system with extremely complex dynamics. The inves-
tigation into the dynamics of multibody systems has been an active topic of re-
search for many years [35, 36, 47, 49, 75, 74, 83, 88, 100]. The complexity and
nonlinearity of this class of such systems grows considerably with its dimension;
so much so that the analysis of walking systems and various other moving multi-
body systems in contact with the environment continue to be on the frontiers
of current research. Fortunately, since the 70's, important discoveries have been
made in combating the \curse of dimensionality" through the development of re-
cursive, symbolic equations for the modeling and evaluation of multibody dynamics
[5, 13, 15, 23, 34, 59, 60, 66, 67, 68, 70, 78]. These approaches greatly reduced the
complexity of handling the equations of such systems, in particular as the number
of bodies in the system increases.
We have multiple goals with our exposition of the robot dynamical models
presented here. One is to bring together recent di�erent approaches made towards
creating new modeling techniques for the dynamics of articulated multibody sys-
tems. The reference [38] compares many previous approaches showing how many
of the recursive algorithms can be shown to be special cases of the same artic-
ulated body algorithm. We continue this line of research by focusing on three
distinct approaches and showing how they complement each other. We call these
the Body, Inertial, and Spatial Representations. A comparative study highlights
their strengths and weaknesses. The development is very general as we discuss the
inverse and forwards dynamics algorithms for general tree-structured systems.
Our development later continues for even more general systems when we
discuss how contact constraints and collision e�ects can be included in the model.
The ability to e�ciently handle these properties is a big step towards reducing
3
complexity. We even introduce a novel reduced dynamics algorithm which can
work with the complete dynamics on a smaller dimensional space.
Another important asset of our model lies in the ability to obtain closed-
form expressions for the derivatives of the various symbolic matrix operators. Jain
and Rodriguez �rst used these for the construction of recursive expressions for the
linearization of the dynamics [42], then later for the development of the normalized
robot dynamics in [44]. We list here several novel extensions which we make in the
use of these derivatives and in other uses of the dynamical algorithms:
1. Many derivations are new and have been made more concise and generalized
to multiple degree of freedom joints.
2. An explicit treatment of the gravitational forces is given which was not present
in previous works.
3. A new reduced dynamics algorithm is presented for cases when the multibody
system is in contact with the environment. The reduced equations of motion
which account for the contact constraints can be evaluated without actually
constructing them.
4. A new concise expression for the Coriolis terms in the dynamics is given which
allows the well-known quantity in robotics ( _M� 2C) to be skew-symmetric.
5. We formalize the use of the gravity terms in taking partial derivatives of the
dynamics with respect to the position and velocity variables.
It is worth pointing out the value of at least two of the above items.
In numerical optimal control to be discussed later, there exist some approaches
which require the partial derivatives of the state equations with respect to the state
variables. These equations can be an obstacle to taking this approach as obtaining
them symbolically can be quite a large task. The availability of e�cient, recursive
algorithms for the calculation of the adjoint equations in optimal control methods,
therefore, can be of tremendous value. This possibility for robot control problems
4
has of yet not been pointed out in the literature. The other very valuable tool which
can greatly reduce complexity is the reduced dynamics algorithm. Constructing
the reduced dynamics can also be a formidable task. The ability to evaluate them
without explicitly constructing them can signify a large computational savings.
I.A.1 Some Previous Work on Dynamics Algorithms
Rodriguez, Jain, and Kreutz-Delgado have presented many important ap-
plications of this modeling approach in previous years. They used these tools for
operational space control design [51], the analysis of exible and underactuated
systems [40, 41], the multi-arm grasp and manipulation of objects [45], and base-
invariant symmetric dynamics of free- ying manipulators [43]. Wendlandt and
Sastry used a similar modeling approach for designing a workspace controller for
balancing a planar biped [99]. Finally, Hardt and Kreutz-Delgado [30] have recently
used these modeling tools together with a backstepping approach for the design of
a new nonlinear adaptive disturbance attenuating controller.
One can group the various multibody formalisms loosely into two groups {
recursive and nonrecursive. For low dimensional systems, the nonrecursive methods
are certainly more e�cient in the calculation of the dynamics. Some examples of
these systems are NEWEUL, MESA VERDE, ADAMS, and DADS [49, 88].
Recursive techniques were apparently �rst developed by Vereshchagin in
1974 [93]. Armstrong then later developed a variation for spherical joints [2]. Feath-
erstone introduced an operator-based decomposition of the dynamics based on screw
and motor calculus and its application to rigid body motion [23]. Featherstone's
model gave considerable insight into the equations of motion, and it is the basis for
one of the representations discussed in this paper.
Both Rodriguez [76] and Brandl [13] then generalized the so-called artic-
ulated body algorithm to handle arbitrary joints. Rodriguez's formulation of the
forward dynamics algorithm, however, was based on a novel factorization of the
mass-inertia matrix which led to a very useful new factorization of the inverse mass
5
matrix. This discovery was inspired by similarities with classical Kalman �ltering
and estimation algorithms. Jain discusses in [38] how all of these approaches can
be interpreted as variations of the same articulated body algorithm.
In the past few years, there has been a lot of interest in exploring Lie group
interpretations of robot dynamics [15, 55, 70, 66]. Most attention has been con-
strained to a body coordinates representation of the dynamics, though the Spatial
Representation presented by Featherstone also has a similar Lie group interpreta-
tion as well as having computational advantages when calculating the dynamics.
The potential of using di�erential geometric tools together with an appropriate
model of the system in analyzing robotic systems are numerous, though we do not
discuss them here. We will focus on simple algebraic manipulations of the dynamics
including total and partial derivatives of the matrix operators.
I.B Nonlinear H1 and Numerical Optimal Control
The continued popularity of H2 and H1 control methodologies is in large
part due to their very good stabilization and robustness properties. There are many
types of control problems which �t naturally into the H2 or H1 framework such as
stabilization problems, trajectory tracking, risk-sensitive problems, and disturbance
attenuation. In the nonlinear regimes, computational methods for solving these
problems are still in their early stages, and there is no generally accepted format
for the best approach to solving them. We �rst discuss in Section V.D a variety of
existing approaches to solving these problems in some computational fashion. We
then pursue two such approaches for solving nonlinear H2 and H1 control problems
which we feel are well-suited to handling saturation and other constraints of the
problem, are e�cient, and can accomodate higher dimensional systems to some
extent.
The approach we outline is a combination of optimization and multiple
shooting algorithms to solve for a good approximation of the value function of the
6
system throughout the state space. From this, one may readily de�ne a closed-
loop control law which is the numerical solution to the nonlinear H2 or H1 control
problems. We illustrate these methods �rst in the test case of a swinging pendulum.
The underlying purpose in the presentation of the pendulum test case is to clarify
the use of the proposed numerical approach with a simple example. Important
subtleties of numerical optimal control also become very evident even in this case.
For example, there may exist initial conditions of the system for which there exist
two completely di�erent, equally optimal solutions. This can lead to situations
where a small change in the initial conditions can lead to radically di�erent solution
trajectories.
I.C Case Study: Jet Engine Compressors
As a practical application of our numerical procedure, we apply these
numerical methods to the control of a jet engine compressor. This system is char-
acterized by severe nonlinearities, most notably rotating stall and also surge. The
3-D Moore-Greitzer model is used as a basis for studying the compressor system,
and we use throttle feedback control to robustly stabilize the system under the
constraints and performance criteria imposed. We calculate the solution of the
nonlinear H2 control problem subject to rate saturation constraints and discuss
considerations and di�erent approaches to the problem in light of undesirable tran-
sient behavior which can arise. The transient behavior of special concern is a large
drop in the plenum pressure rise , and these are punished by the H2 performance
criterion. Our studies also show the strong in uence rate saturation has on this
system. The performance of our �nal controller is then graphically displayed with
contour plots of the resulting value function and drops in the plenum pressure in dif-
ferent operating regions. This work represents the �rst time H2 optimal controllers
were computed for this compressor problem.
7
I.C.1 Some Previous Work on Jet Engine Compressor Control
Much research conducted lately has used throttle feedback and the 3-
D Moore-Greitzer model to control the severe instabilities present in a jet engine
compressor. Liaw and Abed [58] used a throttle-based linear feedback law to develop
a bifurcation-based controller. This control law required sensing of only one of the
state variables, namely the squared amplitude of rotating stall, and it had local
stability results. Eveker, Gysling, Nett, and Sharma [21] later extended this analysis
to design a controller with a much larger operating region. It required, however,
measurement of the time derivative of the mass ow coe�cient _�. Krstic et al.
[53] then developed a backstepping controller which guaranteed global stability,
and it had less restrictive measurement requirements by only requiring sensing of
the mass ow � and the plenum pressure rise . It was even later shown [52]
that this controller is inverse optimal with respect to a performance index. This
result implies robustness and stability margins for the system. Additional other
alternative approaches to controlling the jet engine compressor may be found in
[4, 6, 11, 61, 69]. The results in this paper were �rst announced in [30].
The previous research mentioned did not expressly consider the e�ects
that rate saturation might have on the problem. However, there is new research
focusing primarily on this issue. In [98], a local approximation of the Moore-
Greitzer dynamics is taken at the equilibrium in order to obtain a system of reduced
dimension. The reduced system and controller dynamics are then studied together
using phase space techniques to understand the e�ects of saturation constraints.
Two other methods for handling rate limits in other nonlinear settings by scheduling
or switching linear control laws are found in [56] and [90].
I.D Case Study: Minimum Energy Biped Walking
Many researchers have studied the problem of controlling a walking biped
robot and the associated path planning problem of generating optimal periodic
8
trajectories for the walking motion. Several of the past innovations and recent
contributions are found in the references [17, 16, 27, 25, 46, 64, 80, 81, 85]. Due
to the complexity of the problem, compromising simplifying modeling assumptions
were often made to make it more tractable. The main di�culty, aside from the
complexity of the dynamics which might exist for a 5-link biped robot, is that a
di�erential algebraic system exists with the contact of the feet with the ground.
This work is the �rst to solve the problem of minimum energy gait generation for
biped robots using a complete dynamical model.
The ability to solve this problem depended upon the use of our recursive,
symbolic algorithms for calculating the dynamics of multibody systems, the reduced
dynamics algorithm for a biped system under contact constraints, in combination
with our approach to numerical optimal control using the software DIRCOL. The
methods used were able to handle well the high degree of nonlinearity and the
relatively high dimensionality characteristic of this problem. We explicitly account
for all contact constraints and collision e�ects. We then explore the underactuated
and actuated case with and without the use of ankle torques, the use of impulsive
lifto� forces, and variations with respect to step size and velocity.
I.D.1 Some Previous Work on Biped Locomotion
Some of the early work done by Kajita had a surprising amount of success
by modeling the biped dynamics as that of an inverted pendulum with point masses
[46]. The idea of searching for a passive walking motion which can approximate a
motion which requires only the minimal energy was expressed in the work of McGeer
[64] and later with Goswami et al. [27]. The minimal energy path is desirable for it
exhibits stabilizing, attractive properties. Our experiments have shown that many
walking trajectories naively chosen to approximate walking motion can require a
huge increase in the needed power over that of the optimally calculated minimal
energy trajectories. Other work also investigating minimal energy biped walking
motion, though with simpli�ed dynamical models, may be found in [80, 81].
9
I.E Outline of Dissertation
In chapter II, we begin our discussion of multibody dynamics with the
simple case of a single rigid body. This case allows us to explain the possible choice
of three di�erent representations for the dynamics. We then proceed to extend
the modeling approach to multiple links and ultimately tree-structured systems.
Chapter III extends the model to include contact and collision e�ects as well as the
introduction of contact and impulsive forces. A reduced dynamics algorithm is also
presented in this chapter. Chapter IV completes the e�ort on developing symbolic,
dynamical models by exploring the uses of closed-form symbolic derivatives of the
spatial operators. A Lagrangian derivation of dynamics is given. Then partial
derivatives of the dynamics with respect to the state variables are developed which
may be used for calculating the linearized dynamics, a video motion estimation
algorithm, or recursively calculating the adjoint equations in a nonlinear control
formulation.
The latter part of the paper deals largely with optimal control. The ideas
of nonlinear H2 and H1 control theory are presented in Chapter V together with
a summary of numerical approaches for solving such problems along with our own
choice. We also outline a speci�c numerical scheme for the solution of these prob-
lems and demonstrate them with a pendulum control example. Then in Chapters
VI and VII, we apply our methods to two di�cult important problems. Chapter
VI presents the control of the severe instabilities found in jet engine compressors.
Chapter VII, on the other hand, solves the problem of minimum energy gait gen-
eration for biped robots. Solving this problem requires the synthesis and use of all
the tools developed in this dissertation.
Chapter II
Recursive, Symbolic Robot
Dynamics
II.A A Single Rigid Body
The goal in this section is to develop a symbolic scheme for representing the
dynamics of a single rigid body. Focusing on only a single rigid body also gives us
the opportunity to emphasize in a simpler setting the various related representations
which exist for the dynamics. Though the notation used here borrows heavily from
previous work by Rodriguez, Jain, and Kreutz-Delgado [39, 78], changes have been
made to follow the conventions of many researchers in the �eld [66, 70]. The major
di�erences will be in the reversal of \starred" (adjoint) and \unstarred" operators
and in the use of closed-form derivatives in the derivation. For simplicity, the
starred operators are merely expressed here as a matrix transpose.
The development will proceed by recognizing that the set of equations
de�ning the linear velocities and accelerations may be combined with the angular
equations thereby arriving at a combined 6-dimensional matrix form for the kine-
matic and dynamical expressions. The ensuing matrix operators have time deriva-
tive expressions which may similarly be expressed in terms of other 6-dimensional
or spatial matrix operators. Concise matrix equations are thus obtained which have
10
11
physical and geometrical interpretations.
II.A.1 Dynamics in Inertial Space
We begin by considering the dynamics of a single rigid body with all
quantities represented in an inertial frame. Consider a single rigid body moving
through space. Let !; v;N;F be the angular and linear velocity, moment and
linear force of the rigid body respectively expressed in an inertial frame. Then at
two di�erent points in the body, x and y, the above quantities are related in the
following manner:
!y = !x Nx = Ny + lx;y � Fy
vy = vx � lx;y � !x Fx = Fy(2.1)
where lx;y represents the 3-dimensional vector from x to y, also expressed in an
inertial frame.
A more compact form is obtained with the spatial notation (di�erent from
Featherstone's Spatial Representation [23]) if one de�nes the 6-dimensional spatial
velocity and spatial force as
Vx =
264 !x
vx
375 fx =
264 Nx
Fx
375 ; (2.2)
and de�ne the rigid body transformation operator as
�y;x =
264 I 0
�~lx;y I
375 ; (2.3)
where I 2 R3x3 is the identity operator. The quantity ~l 2 R3x3 is the cross product
matrix generated from the vector l, i.e. for p 2 R3, l�p = ~l p, which when expressed
in coordinates yields
l =
2666664x
y
z
3777775 =) ~l =
2666664
0 �z y
z 0 �x
�y x 0
3777775 (2.4)
12
Then the relations in (2.1) may be expressed as
Vy = �y;xVx ; fx = �Ty;xfy : (2.5)
For future reference, the tilde operator satis�es several useful identities which may
be found in Section A.B in Appendix A.
The dynamics of a rigid body at its center of mass are well known and may
be found in such texts as Wittenburg [100]. In order to de�ne an operator expression
for the dynamics, we �rst de�ne the spatial inertia and the spatial momentum. Let
the subscript C represent the center of mass of the rigid body, m will denote the
mass of the body, and JC is its inertia tensor at the center of mass. Then the spatial
inertia MC and the spatial momentum �C are given by
MC =
264 JC 0
0 mI
375 ; �C =MCVC : (2.6)
For the purposes of calculating the dynamics of the rigid body, we will need expres-
sions for the derivatives of � and M . We will use the spatial cross product operator
in the form introduced by Jain and Rodriguez in [44], where if X = [aT bT ]T ,
Y = [cT dT ]T are spatial vectors then
X � Y = ~XY =
264 ~ac
~bc+ ~ad
375 , with ~X =
264 ~a 0
~b ~a
375 : (2.7)
It can also easily be shown that
~XY = �~Y X (2.8)
~XX = 0 : (2.9)
Featherstone �rst introduced the spatial cross operator (as he calls it) in [23] to
handle general expressions for derivatives of spatial vectors and various other spatial
operators.
13
The inertia operator MC generalizes to any point x in the rigid body. At
the body center of mass, the kinetic energy is given by
KE =1
2V T
CMCVC : (2.10)
The kinetic energy is the same at any point in the body, and we may implicitly
derive a transformation law for M using the fact that VC = �C;xVx,
KE =1
2V T
x�TC;xMC�C;xVx =
1
2V T
xMxVx ; (2.11)
so that the spatial inertia at a point x is given by
Mx = �TC;xMC�C;x =
264 Jx m~lx;C
�m~lx;C mI
375 ;
where Jx = JC�m~lx;C~lx;C is the well-known transformation law of the inertia tensor
[100]. Note that Mx remains symmetric and positive de�nite.
Two important operators which will be valuable in expressing the deriva-
tives of some of the above operators are the spatial cross product operators ~ and
~� computed from the spatial vectors and �,
x =
264 !x
0
375 ; �
y;x=
264 (!y � !x)
0
375 : (2.12)
Note that because of the special structure of x and �y;x, ~x and ~�
x;ywill be
skew-symmetric.
We now develop two important time derivatives of two spatial operators
to be used for later determining matrix equations for the spatial acceleration _V .
This will result in symbolic expressions for the inverse dynamics (the determination
of the applied torques from the generalized velocities and accelerations).
Proposition 1
_�y;x = ~x�y;x � �y;x ~x (2.13)
_Mx = ~xMx �Mx~x (2.14)
14
Proof: The total time derivative of a 3-vector of �xed length is given by the cross
product of the angular velocity of the coordinate frame to which it is attached and
the vector itself. As a result, The total time derivative of lx;y isd
dtlx;y = ~!x lx;y [100].
Using the tilde identity (A.4), we may write the inertial derivative of the rigid body
transformation operator � as
_�y;x =
264 0 0
�~!x~lx;y + ~lx;y~!x 0
375 : (2.15)
The resulting matrix expressions for _� may then be veri�ed through direct matrix
multiplication.
Using the fact that _J = ~!J�J ~!, we may obtain the derivative expression
for Mx:
_Mx =
264 ~!xJx � Jx~!x m(~!x~lx;C � ~lx;C ~!x)
�m(~!x~lx;C � ~lx;C ~!x) 0
375 : (2.16)
The matrix expressions similarly follow through matrix multiplication.
At the center of mass, the spatial force experienced by the rigid body, fC,
may be obtained by taking the total time derivative of the spatial momentum �C,
which is the product of the spatial inertiaMC and the spatial velocity VC [78, 100].
We would like to express this quantity, however, in terms of the spatial velocity
and acceleration re referenced at the joint position x as we ultimately would like
to know the inverse dynamics at this point. This may be done by substituting in
the known transformation for the spatial velocity.
Proposition 2
fC = MC[�C;x _Vx + (~x�C;x � �C;x~x)Vx]
+(~CMC �MC~C)VC
(2.17)
Proof: First observe how _VC relates to _Vx. Recalling that VC = �C;xVx, we have
from Proposition 1 that
_VC = �C;x _Vx + _�C;xVx
= �C;x _Vx + (~x�C;x � �C;x ~x)Vx :
15
Taking the time derivative of the spatial momentum now gives the desired spatial
force:
fC = _�C =MC_VC + _MCVC
= MC_VC + (~CMC �MC
~C)VC : (2.18)
Finally, substituting for _VC gives the �nal expression.
Before continuing, we introduce a useful new identity and simultaneously
the new operator ~Vx which is constructed from applying the spatial cross product
operator to the spatial velocity Vx.
Identity 3
�TC;x
~x�T
x;CMxVx = �~V T
xMxVx (2.19)
Proof: By multiplication of the matrix operators, we obtain
�x;C ~x�T
C;xMxVx =
2666664
�~!xJx~!x +m~!x~lx;Cvx
+(~lx;C~!x � ~!x~lx;C)(�m~lx;C!x +mvx)
�
�m~!x~lx;C!x +m~!xvx
3777775 (2.20)
�~V TMxVx =
264 ~!xJx!x +m~!x~lx;Cvx �m~vx~lx;C!x
�m~!x~lx;C!x +m~!xvx
375 (2.21)
The equality of the two expressions is shown with the use of the tilde identities
found in Section A.B of Appendix A.
If we wish to obtain an expression relating the spatial force, fx, and the
spatial dynamics at a point x away from the center of mass, we simply use the
operator �T to translate fC to fx as pointed out in Eq. (2.5). Using the previ-
ously de�ned derivatives and identities, a concise expression is obtained for this
relationship.
Proposition 4
fx =Mx_Vx � (Mx
~x + ~V T
xMx)Vx (2.22)
16
Proof:
fx = �TC;xfC
= �TC;x
hMC
_VC + (~CMC �MC~C)VC
i= �T
C;xMC
��C;x _Vx + (~x�C;x � �C;x ~x)Vx
�+ �T
C;x(~CMC �MC
~C)VC
= Mx_Vx + �T
C;xMC
~x�C;xVx �Mx~xVx + �T
C;x(~xMC �MC
~x)�C;xVx
= Mx_Vx �Mx
~xVx + �TC;x
~x(�T
C;x)�1MxVx
(2:19)= Mx
_Vx � (Mx~x + ~V T
xMx)Vx
We have used the fact that ~x = ~C , since the angular velocity is the same at any
point in the rigid body (2.1), and that ��1C;x
= �x;C. Equation (2.22) is a key result
which gives a compact expression for the rigid body dynamics and which will later
be used when dealing with multiple rigid bodies. In (2.22), fx represents the net
result of all forces acting on a rigid body, including natural forces (such as gravity)
and applied control forces.
II.A.2 Dynamics in Body Space
Recent research in advanced robotics has begun to make extensive use of
Lie group theory and di�erential geometry [66, 70, 84]. A formalized treatment of
this approach to robotics is found in the recent book [66] where it is shown how
rigid body motion in robotic systems may be expressed as actions on Lie groups.
Using the theory described in [66] and the results we have presented so far, it
can be pointed out that the rigid body transformation operator �y;x is a matrix
representation of the adjoint operator, Ad, on the Lie group of rigid body motion
SE(3). Meanwhile, the spatial cross product operator de�ned in (2.7) represents
the ad operator on its Lie algebra se(3). The book [66] shows how the Body
Representation of the dynamics allows one to make direct associations with elements
of Lie group theory in contrast to the more traditional use of inertial coordinates.
17
In the case of a single rigid body, the dynamics expressed in body space
have the same structure as in inertial coordinates, and may be obtained from inertial
coordinates merely by multiplication of a 6�6 matrix with a rotation matrix in the
diagonal blocks. This transforms the dynamics to a form represented with respect
to a coordinate frame �xed in the rigid body. A more concise notation is obtained,
as was shown by Park et al. [70] and Ploen [72], if one expresses the dynamics in
terms of the local time derivative of the spatial velocity rather than the inertial
time derivative. It is a standard result [100] that for an arbitrary vector a, if�
a
represents the local derivative of a with respect to the body and ! is the angular
velocity of the rigid body, that _a =�
a + !�a. This leads to the following expression
for the spatial velocity represented at a point in the rigid body which is valid in
either inertial or body coordinates,
_Vx =�
Vx + ~xVx : (2.23)
Substituting for _Vx in (2.22) gives
f bx=M b
x
�
V b � ~V bT
xM b
xV b
x; (2.24)
where the superscript b is used for clarity in order to emphasize that the quantities
are expressed with respect to a body-�xed coordinate frame. Though the derivation
of the body coordinate dynamics (2.24) seemingly di�ers from that found in [70],
it is merely a stacked representation of the dynamics derived in [70] via Lie group
techniques.
II.A.3 Dynamics in the Spatial Representation
Featherstone introduced in his landmark book [23], a screw-theory based
description of multibody dynamics. In the book [66], it was later shown how this
approach also has Lie group interpretations which parallel the Body Representa-
tion, and these coordinates were similarly given the name spatial coordinates in
[66]. For this reason, we denote the screw-theory based approach as the Spatial
18
Representation (not to be confused with 6-dimensional spatial vectors or spatial
quantities previously de�ned) even though the dynamics are represented with re-
spect to an inertial frame. The real di�erence between this approach and the
inertial or body space approaches lies in calculating the dynamics at the origin of
the inertial coordinate system, rather than at a point in the rigid body.
To further illustrate the di�erences between the various approaches, let
x be a point in the rigid body at which a coordinate frame Fx is attached. Also,
let o be the point at the origin of the inertial coordinate frame Fo. Then the body
space approach calculates the dynamics of the rigid body at x and represents them
with respect to Fx, the inertial space approach also calculates the dynamics at x,
but represents them with respect to Fo, and the spatial representation performs the
calculation at o and with respect to Fo.
Inertial time derivatives of spatial operators in the Spatial Representation
will also di�er from the inertial space approach, since the operators will represent
physical quantities at two di�erent points in space. Featherstone shows that if X
is any 6-dimensional spatial vector [23], then in the Spatial Representation it will
satisfy
_X =�
X + ~V X : (2.25)
A direct consequence of this relationship is that
_V =�
V ; (2.26)
since ~V V = 0 as pointed out in Eq. (2.9).
It is important not to confuse this result with that found in Eq. (2.23).
The spatial velocity V as found in the Inertial and Body Representations di�er
only by a change of coordinates. In the Spatial Representation, V also re ects a
translation away from the joint position, and consequently, its time derivative will
also have a di�erent interpretation than in the inertial case.
Remark 5 The choice of representation will in uence the result of taking deriva-
tives of the symbolic, matrix operators.
19
One may consider it confusing that these quantities are given the same
symbol in this dissertation (i.e. simply V rather than Vx or Vo), Though in coor-
dinate form, they are fundamentally di�erent. However, representing the velocity
at a di�erent point other than the joint position is akin to a change in coordi-
nates and, as the equations we are developing are inherently independent of these
choices, we do not make a distinction. In fact, one can obtain the Spatial Represen-
tation version of V from the Inertial Representation version using the rigid body
transformation operator as in (2.5),
Vo = �o;xVx ; (2.27)
where o and x represents the origin and body joint position respectively.
The spatial inertia matrix M when represented in a symmetric form in
the Spatial Representation (unlike Featherstone) has the derivative
Proposition 6
_M = �~V TM �M ~V : (2.28)
Proof: The result may be shown through direct multiplication of the various
components of the matrix operators.
Remarkably, in the Spatial Representation, one does not need to calculate
the dynamics at the center of mass of the rigid body as one must do when working
with the Inertial and Body Representations. The net force acting on the rigid body
f s may be obtained directly by taking the time derivative of the spatial momentum
about the origin of the inertial coordinate system, M s
oV s
o. Using the superscript s
for clarity to denote the Spatial Representation, we have
f s =d
dt(M s
oV s
o) =M s
o_V s
o+ _M s
oV s
o
= M s
o_V s
o� ~V sT
oM s
oV s
o(2.29)
20
II.B Multiple Rigid Bodies in Inertial Space
We now discuss how the previous results on single rigid bodies may be
extended to multiple rigid bodies. In Figure 2.1 is shown a serial multibody chain
of links, where the (i� 1)th
and ith links are coupled together via the ith joint.
The joint locations Oi are additionally important since it is at these points that
we will be calculating the dynamics of the rigid bodies for the Inertial and Body
Representations. A frame Fi will be rigidly attached to the ith link at Oi, and the
vector joining Oi�1 and Oi is li�1;i. The rate of change of the frame Fi with respect
to the frame Fi�1 determines the relative angular and linear velocities. The spatial
vector which consists of these quantities is called the relative spatial velocity V �
i
(de�ned at the point Oi).
i-th link(i-1)th link
Base
x x
i-th joint
l(i-1,i)
F(i-1)
O(i-1)
C(i-1) O(i)
F(i)C(i)
l(i,C)
Tip
Figure 2.1: A serial multibody chain
The 6-dimensional relative spatial velocity between links (i � 1) and i,
V �
i, may be obtained from the joint angle velocity _�i via a linear transformation
Hi : Rm ! R6, m 2 1; : : : ; 6, for a \ at" representation where m equals the
motion degrees of freedom of the joint. We call this a \ at" representation as we
only consider the possibility of having an equal number of generalized position and
velocity variables as the number of degrees of freedom and, moreover, the velocity
variables are the time derivatives of the positions. In this setting, a maximum of 6
degrees of freedom are possible for the joint, whereby three are rotational and three
are translational. Other possibilities include in the case of a 3 dof rotational joint
a 4-dimensional singularity-free quaternion position vector and a non-integrable
21
angular velocity vector for the generalized velocity. These extensions require addi-
tional kinematic transformations to occur before and after the dynamical recursions
but are straightforward. We omit this development in the interest of brevity and
focus.
Constructing Hi in the Inertial Representation is most easily done by
transforming the Body Representational form of Hb
i, where it has a constant form.
We use here the b superscript for clarity. Most commonly, in body coordinates, each
column of Hb
iis a di�erent 6-dimensional unit vector [eT
i03]
T or [03 eTi]T in the
direction of the axis of rotation or translation, where the rotational axes come �rst.
For example, a joint with one rotational dof along the z-axis and two translational
dof along the x- and y-axes will have the form,
Hb
i=
26666666666666664
0 0 0
0 0 0
1 0 0
0 1 0
0 0 1
0 0 0
37777777777777775
(2.30)
The link i angle velocity vector _�i will similarly have its velocity components ordered
in a similar way as Hb
i.
Remark 7 We will continue on the assumption that the state vectors will always
be stored in body coordinates.
Though it is possible to store the state in inertial coordinates, this alter-
nate choice will have an in uence on the dynamical calculations which we choose
not to explore here. Operating Hi on _�i gives the body relative spatial velocity
V �b
iwhich may be transformed to the Inertial Representation merely by rotating
coordinates. De�ne now the spatial rotational operator �Rias
�Ri=
264 Ri 0
0 Ri
375 ; (2.31)
22
where Ri maps body i coordinates to inertial coordinates. Then, since V �
i= �Ri
V �b
i,
it becomes evident that the correct transformation rule for Hi is
Hi = �RiHb
i(2.32)
The adjoint HT
iserves the dual role of mapping the spatial force at the
ith joint fi to the applied work-producing joint forces ui at the ith joint:
V �
i= Hi
_�i ; ui = HT
ifi : (2.33)
We now formulate the dynamics of a chain of rigid bodies. Since each link
is rigid, it su�ces to develop the equations of motion at a single reference point on
the link, which we take to be the joint location Oi. The spatial velocity Vi at Oi on
the ith link consists of the sum of the contributions from the motion of the (i� 1)th
link transformed to the point Oi plus the additional relative velocity V�
ibetween
the two links generated by the joint velocity _�i. Thus,
Vi = �i;i�1Vi�1 +Hi_�i : (2.34)
In Section II.A.1, we introduced the derivative of the transformation op-
erator �. With multiple links, this same operator is now used to also propagate
spatial velocities across joints, and its adjoint does the same with spatial forces.
With prismatic joints, however, the distance between successive joints can vary
over time. This e�ect must be accounted for so that we reintroduce a more general
derivative for �. We also write down the derivative of H to be later used in the
derivation of the spatial accelerations.
Proposition 8
_�i;i�1 = �~V �
i�i;i�1 + ~i�i;i�1 � �i;i�1 ~i�1 (2.35)
_Hi = ~iHi : (2.36)
23
Proof: Recall that
�i;i�1 =
264 I 0
�~li�1;i I
375
The total time derivative of li�1;i is given by d
dtli�1;i = _li�1;i + ~!i�1 li�1;i = �(vi �
vi�1) + ~!i�1 li�1;i [100]. Using the tilde identity (A.4), we may write the inertial
derivative of the rigid body transformation operator � as
_�i;i�1 =
264 0 0�
� (~vi � ~vi�1) + ~!i�1~li�1;i + ~li�1;i~!i�1
�0
375 : (2.37)
Making the observation that
V �
i=
264 (!i � !i�1)
(vi � vi�1)
375 ; (2.38)
the �nal result may then be shown through direct matrix multiplication.
The time derivative ofHi is best obtained by di�erentiating (2.32). Noting
that in inertial coordinates _R = ~!R, the time derivative of �Riis
_�Ri= ~ �Ri
: (2.39)
The result then follows.
Now, di�erentiating (2.34) gives the spatial acceleration for the ith link.
Proposition 9
_Vi = �i;i�1 _Vi�1 + (~i � ~V �
i)Vi � �i;i�1~i�1Vi�1 +Hi
��i (2.40)
Proof:
_Vi = �i;i�1 _Vi�1 + _�i;i�1Vi�1 + _Hi_�i +Hi
��i
= �i;i�1 _Vi�1 +��~V �
i�i;i�1 + ~i�i;i�1 � �i;i�1 ~i�1
�Vi�1 + ~iHi
_�i +Hi��i
= �i;i�1 _Vi�1 + (~i � ~V �
i)(�i;i�1Vi�1 +Hi
_�i)� �i;i�1~i�1Vi�1 +Hi��i
= �i;i�1 _Vi�1 + (~i � ~V �
i)Vi � �i;i�1 ~i�1Vi�1 +Hi
��i
24
We may also obtain a recursive expression for the spatial forces. The spa-
tial forces are a combination of the constraint forces fi+1 imparted by the (i+ 1)th
link at Oi+1 and the forces generated due to the motion of the ith link as given in
Eq. (2.22). Thus, the recursions for the spatial forces and momenta become
fi = �Ti+1;ifi+1 +Mi
_Vi � (Mi~i + ~V T
iMi)Vi
ui = HT
ifi (2.41)
All together, the Newton-Euler recursive equations of motion for the serial
multibody chain with N links are8>>>>>>>>>>>><>>>>>>>>>>>>:
V0 = 0; _V0 = 0
for i = 1; :::;N
Vi = �i;i�1Vi�1 +Hi_�i
_Vi = �i;i�1( _Vi�1 � ~i�1Vi�1) + (~i � ~V �
i)Vi +Hi
��i
end loop
(2.42)
8>>>>>>>>>>>><>>>>>>>>>>>>:
fn+1 = 0
for i =N; :::;1
fi = �Ti+1;ifi+1 +Mi
_Vi � (Mi~i + ~V T
iMi)Vi
ui = HT
ifi
end loop
(2.43)
Note that when there is a mobile base or there are non-zero tip forces, the initial-
izations for V; _V ; f may be appropriately modi�ed.
A convenient way to express the equations of motion for the entire system
is in stacked form [78, 44]. For example, the spatial vectors are written in one large
vector where the ith element of the vector is the spatial vector corresponding to
the ith link. If the total degrees of freedom of the system is equal to d, then the
dimensions of the stacked operators are:
f = colffig 2 R6N V = colfVig 2 R6N
u = colfuig 2 Rd _V = colf _Vig 2 R6N
_� = colf _�ig 2 Rd �� = colf��ig 2 R
d
(2.44)
25
Similarly, we de�ne
M = diagfMig 2 R6N�6N H = diagfHig 2 R6N�d
~ = diagf~ig 2 R6N�6N ~V � = diagf~V �
ig 2 R6N�6N
~V = diagf~Vig 2 R6N�6N
(2.45)
Finally,
E� =
0BBBBBBBBBBBB@
0 0 � � � 0 0
�2;1 0 � � � 0 0
0 �3;2 � � � 0 0...
.... . .
......
0 0 � � � �N;N�1 0
1CCCCCCCCCCCCA
� = (I � E�)�1 = I + E� + � � �+ EN�
=
0BBBBBBBB@
I 0 � � � 0
�2;1 I � � � 0...
.... . .
...
�N;1 �N;2 � � � I
1CCCCCCCCA
(2.46)
with �i;j = �i;i�1 � � ��j+1;j.
The recursive equations of motion (2.42), (2.43) may now be written as
V = �H _�
_V = �H �� + ~V � �~V �V (2.47)
f = �TM _V � �T (M ~ + ~V TM)V
u = HTf
Combining (2.47) together, we may succinctly express the Newton-Euler equations
of motion as
M�� + C = u (2.48)
where
M = HT�TM�H C = HT�T [�M�~V � � ~V TM ]V (2.49)
The expression for the mass-inertia M, M = HT�TM�H, is called the Newton-
Euler factorization ofM. This compact expression for C was �rst given in a similar
form in [42].
Remark 10 An important result is that these expressions forM and C are invari-
ant with respect to the Inertial, Body, or Spatial Representations.
26
The invariance of Representation gives one the freedom to choose which method to
calculate the dynamics and calculational considerations will generally take prece-
dence. We will see that in this regard, the Body and Spatial Representations are
both superior to the Inertial Representation.
II.C Multiple Rigid Bodies in Body Space
If one expresses the equations of motion in local body, certain operators
take a simpler form thereby making the recursions simpler. Also, direct associations
may be made to Lie group theory as were done in [70], thereby making available
many tools in di�erential geometry which can be valuable in future research in
dynamics and control design.
As the spatial forces and velocities are propagated from link to link, now a
coordinate transformation must also take place before the e�ects from neighboring
links may be included. The recursive equation for Vi still has the same form as in
(2.34),
Vi = �i;i�1Vi�1 +Hi_�i ; (2.50)
yet now the construction of the coordinate form of the various matrix operators
will di�er.
In this section, all quantities will be expressed in local body coordinates
which implies that Hi is constant and takes a simple form as discussed in Section
II.B with example (2.30). The rigid body transformation operator may now be
expressed in one of two ways:
�i;i�1 =
0B@ Ri 0
~li;i�1Ri Ri
1CA =
0B@ Ri 0
�Ri~li�1;i Ri
1CA : (2.51)
The 3 � 3 matrix Ri is a rotation matrix which maps a vector from the (i� 1)th
link frame Fi�1 to Fi in the ith link. The position vector li;i�1 is the position of
the ith joint, Oi relative to the (i� 1)th joint, Oi�1, expressed in the frame Fi�1.
27
The vector li;i�1, on the other hand, expresses the location of Oi�1 relative to Oi
with respect to the frame Fi. As described in [66, 70], this quantity is the adjoint
operator of the inverse transformation in SE(3) from link (i� 1) to link i.
In the Body Representation of the dynamics, a natural derivative to use is
the local time derivative. It is not explicitly stated in [70], but the local derivative
of the spatial velocity V is used in that reference for the inverse dynamics which
gives a more compact symbolic representation of the dynamics. We present here an
alternative derivation of the dynamics using local time derivatives without the use
of Lie group theory. We �rst introduce the derivative of the Body Representational
version of the � operator.
Identity 11�
�i;i�1 = �~V �
i�i;i�1 (2.52)
Proof: The local derivative of the vector from the (i � 1)th joint to the ith
joint, li�1;i, will be the di�erence of the linear velocity of link i from link (i � 1),
�l i�1;i =(i�1)vi � vi�1. Since li�1;i is represented in link (i � 1) coordinate frame,
its local derivative will similarly be in this frame. This is indicated by the (i� 1)
superscript next to vi. Since for any vector x, gR~x = R~xRT , then RT
i~vi � ~vi�1 =
~vi � (i)~vi�1Ri. Derivatives of rotation matrices also satisfy, �R = �(f!i � g(i)!i�1)R,
so that
�
�i;i�1 = �
264 (~!i � (i)~!i�1) 0
(~vi � (i)~vi�1) (~!i � (i)~!i�1)
375 �i;i�1
In stacked form, we may now write down the following identities:
Identity 12
�
E� = �~V �E� (2.53)
�
� = ��
E� � = ��~V �E�� (2.54)
�
V = ��~V �V + �H �� (2.55)
28
Proof: The �rst result is merely a stacked form of Eq. (2.52). From Identity
A.9 in the Appendix, it follows that �E� = � � I. Taking the local derivative of
both sides of this equality gives�
�(I � E�) = ��
E�, so that the local derivative of
the stacked � operator is given by ��
E� �. For the third equation, we take a local
derivative of the spatial velocity V = �H _�.
�
V = ��~V �E��H �� + �H ��
= ��~V �V + �~V �V � + �H ��
The last equation follows from Identities 2.9 and A.9 and the de�nition of V � in
(2.33).
We now have the following fully equivalent stacked and recursive equations
for the Body inverse dynamics:
Stacked Recursive
V = �H _��
V = ��~V �V + �H ��
8>>>>>>>>>>>><>>>>>>>>>>>>:
V0 = 0; _V0 = 0
for i = 1; :::;N
Vi = �i;i�1Vi�1 +Hi_�i
�
Vi = �i;i�1�
Vi�1 �~V �
iVi +Hi
��i
end loop
(2.56)
The spatial forces may be calculated as a combination of the constraint forces
present in the (i+ 1)th link and the forces generated due to the motion of the ith
link. The latter was previously given in (2.24), while the spatial force of the (i+1)th
29
link is propagated via the rigid body transformation operator �Ti+1;i.
Stacked Recursive
f = �TM�
V ��T ~V TMV
T = HTf
8>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>:
fN+1 = 0
for i = N; :::;1
fi = �Ti+1;ifi+1 +Mi
�
Vi
�~V T
iMiVi
ui = HT
ifi
end loop
(2.57)
The set of equations (2.56) and (2.57) may be found in [70], where the
major di�erences lie in that the nested ad and Ad operators are expressed here
with one tilde operator. We �nd the above notation more concise and easier to
work with, though it does not express directly its Lie group interpretations. If one
substitutes for�
V to place these equations in terms of _V using Eq. (2.23), one will
have the same structural form of the equations as in the inertial case (2.42), (2.43),
and (2.47), though the dynamics will still be represented with respect to di�erent
coordinate systems. Thus, recursing on the local derivatives of V allows one to
express the equations of motion in a simpler symbolic form.
Remark 13 When using the local spatial acceleration for other calculations, one
must keep in mind that this quantity has a di�erent interpretation than the inertial
spatial acceleration _V .
Enforcing the linear components of the local spatial acceleration at a tip in
contact with the ground to be zero will not have the same e�ect as doing the same
with the inertial spatial acceleration. This fact will be of importance later with the
Contact Algorithm to be discussed in Chapter III. That algorithm may be used in
a situation where we require that certain components of the spatial acceleration of
a branch tip in contact with the ground be zero. A Body approach can still be used
to calculate the dynamics by noting that the inertial spatial accelerations rotated
30
to body coordinates satis�es a recursion identical to that found in the Inertial
Representation (2.42) and (2.43).
II.D Multiple Rigid Bodies with the Spatial Representa-
tion
The symbolic form of the dynamics in the Spatial Representation has
a simple form similar to the Body Representation. The interesting aspect of this
particular approach is that since the coordinates are all in the inertial frame and the
dynamics are all represented at the origin (instead of at each link's joint position),
the transformation operator �i;i�1 to propagate the spatial velocities and spatial
forces is just the identity matrix. The only additional identity that will be needed
to complete the Spatial inverse dynamics will be the following:
Proposition 14
_H = ~V H (2.58)
Proof: From Featherstone [23], we have for the derivative which transforms
spatial velocities from the Body Representation to the Spatial one:
_�o;i = ~V �o;i (2.59)
Similar to Eq. (2.32), Hi can be constructed from the constant Body form of Hi.
Hi = �o;iHb
i(2.60)
Taking the derivative of this last expression gives _Hi = ~ViHi. The �nal result is
then the stacked version of _Hi.
Using the identity 2.8 and the previously de�ned derivatives, it is possible
to establish the Spatial inverse dynamics algorithm:
V = �H _�
31
_V = �H �� � �~V �V (2.61)
f = �TM _V � �T ~V TMV
u = HTf
The rigid body transformation operator has its simplest form in the Spatial
Representation where in stacked notation we have,
E� =
0BBBBBBBBBBBB@
0 0 � � � 0 0
I 0 � � � 0 0
0 I � � � 0 0...
.... . .
......
0 0 � � � I 0
1CCCCCCCCCCCCA
� =
0BBBBBBBB@
I 0 � � � 0
I I � � � 0...
.... . .
...
I I � � � I
1CCCCCCCCA
(2.62)
As a result of this simple form, _� = 0 = _E�. As will later be seen, this yields simpler
expressions in di�erent applications involving derivatives of the dynamics.
II.E Comparison of Dynamical Representations
In the Inertial Representation, we calculated the derivative of each link's
spatial momentumMV at its center of mass to obtain the dynamics at that point,
then we transformed the dynamics to the joint position. If one is to choose a point
in each link from which to recurse on, then doing this with the joint positions has
some computational advantages. The Inertial Representation has as its principal
virtue that all of its spatial quantities when represented in coordinate form are in
inertial coordinates and provide a direct physical interpretation of the system.
The Body approach presents a more e�cient method of calculating the
dynamics as the joint projection operator H is constant and is generally constant
in body coordinates. Similarly, the spatial inertiaM is constant in body coordinates
and need only be calculated once, which may be done o�ine before any dynamical
algorithms are undertaken. Finally, in the calculation of the inverse dynamics, a
32
Representation Notation of this Work
Spatial Velocity Recursion
Body: Vi = �i;i�1Vi�1 +Hi_�i
Inertial: Vi = �i;i�1Vi�1 +Hi_�i
Spatial: Vi = Vi�1 +Hi_�i
Rigid Body Transformation Operator
Body: �i;i�1 =
0B@ Ri
i�1 0
�Ri
i�1~li�1;i Ri
i�1
1CA
Inertial: �i;i�1 =
0B@ I 0
�~li�1;i I
1CA
Spatial: �i;i�1 =
0B@ I 0
0 I
1CA
Spatial Acceleration Recursion
Body:�
Vi= �i;i�1�
Vi�1 �~�i;i�1Vi +Hi
��i
Inertial: _Vi = �i;i�1 _Vi�1 + ~iVi
��i;i�1~i�1Vi�1 � ~�i;i�1Vi +Hi
��i
Spatial: _Vi = _Vi�1 � ~�i;i�1Vi +Hi
��i
Spatial Force Recursion
Body: fi = �Ti+1;ifi+1 +Mi
�
Vi �~V T
iMiVi
Inertial: fi = �Ti+1;ifi+1 +Mi
_Vi � (Mi~i + ~V T
iMi)Vi
Spatial: fi = fi+1 +Mi_Vi � ~V T
iMiVi
Table 2.1: Comparison of Gravity-Free Inverse Dynamics Representations (Current
Notation).
simpler construction is obtained as shown in [70] by recursing on the local derivative
of the spatial velocities.
The Spatial Representation is also calculationally e�cient and �ts into the
Lie algebraic framework as does the Body Representation. The di�erence of this
approach lies in representing the dynamics with respect to an inertial coordinate
33
Representation Originally Presented Notation
Spatial Velocity Recursion
Body: Vi = Adf�1i�1;i
(Vi�1) + Si _xi
Inertial: V (k) = ��(k + 1; k)V (k + 1) +H�(k) _�(k)
Spatial: vi = vi�1 + si _qi
Rigid Body Transformation Operator
Body: Adf�1i�1;i
=
0B@ � 0
[b]� �
1CA
Inertial: ��(k + 1; k) =
0B@ I 0
�~l(k + 1; k) I
1CA
Spatial: P XO =
0B@ E 0
Er�T E
1CA
Spatial Acceleration Recursion
Body: _Vi = Si�xi +Adf�1i�1;i
( _Vi�1) + adAd
f�1i�1;i
( _Vi�1)(Si _xi)
Inertial: �(k) = ��(k + 1; k)�(k + 1) +H�(k)��(k) + a(k)
Spatial: ai = ai�1 + vi�si _qi + si�qi
Spatial Force Recursion
Body: Fi = AdTf�1i;i+1
(Fi+1) + Ji _Vi � adTVi(JiVi)
Inertial: f(k) = �(k; k � 1)f(k � 1) +M(k)�(k) + b(k)
Spatial: fi = fi+1 + Iiai + vi�Iivi
Table 2.2: Comparison of Gravity-Free Inverse Dynamics Representations. (Origi-
nally Presented Notation)
system as previously described, except the dynamics of each rigid body is repre-
sented about the same point, the origin of the inertial coordinate system. This is
in contrast to the two previously outlined approaches where the dynamics of the
rigid bodies are calculated at their respective joint positions. The advantage of
representing all the dynamics about the same point is that the rigid body transfor-
mation operator �i;i�1 becomes the identity. This then compensates for the extra
34
cost involved in having to transform H and M to the inertial frame each time the
dynamics are calculated.
In spite of its nonintuitive interpretation, the Spatial Representation has
several appealing characteristics. In addition to it being calculationally e�cient,
it does have the advantage that all quantities are represented with respect to the
same coordinate frame and, hence, can be more easily analyzed and compared.
Also, whereas in the Inertial and Body approaches, the dynamics are obtained
by taking the time derivative of the spatial momentumMCVC at each rigid body's
center of mass and then propogating the forces to the joint positions, we do not have
this restriction using the spatial coordinate system. One may take the derivative
directly of the spatial momentumMoVo of each rigid body represented at the origin
of the inertial coordinate system to obtain the dynamics, rather than taking the
time derivative at the center of mass and then transforming the expression to the
joint position.
In Table 2.1, we list together the inverse dynamics algorithms for the three
Representations in order to highlight their similarities. The originally presented
notation of these algorithms are shown in Table 2.2. The Body notation stems
from the work by Park et al. [70], the Inertial notation by Rodriguez et al. [78],
and �nally the Spatial notation by Featherstone [23].
II.F Dynamics with Gravity
Until now, we have omitted any reference to the important e�ect of gravity
in the dynamical equations. Commonly, the equations of motion of a mechanical
system in a uniform gravitational �eld are represented in the form,
M�� + C + G = u ; (2.63)
or equivalently
M�� + C = u+ ug : (2.64)
35
These equations contain, in addition to the mass-inertia matrix M and the vec-
tor of Coriolis and centrifugal forces C found in (2.48) and de�ned in (2.49), the
vector of gravitational forces G which correspond to the vector ug = �G of actual
joint applied forces resulting from the e�ects of gravity. We may also develop a
matrix operator expression for G which will allow one to calculate and formally
introduce it into the equations for the inverse and forward dynamics. This term
serves an important role in many di�erent applications involving the calculation of
the dynamics, and an explicit representation for it is often useful.
A standard approach to simulating the e�ect of gravity at the earth's
surface on the multibody system is to give the inertial frame an upward pseudo-
acceleration equivalent to the actual downward acceleration due to gravity [60].
This stems from the so-called Principle of Equivalence. This approach works equally
well when the base of the multibody system is �xed and its coordinate frame is the
same as the inertial one or when the base is not �xed and the multibody system
is free- ying and, thus, accelerating downward relative to the inertial frame as a
consequence of gravitational attraction.
In the inverse dynamics, we begin with the joint accelerations ��, which
are the true accelerations re ecting already the in uence of the gravitational forces.
The next step in the inverse dynamics algorithm is to construct the spatial acceler-
ations. Introducing �� into any of the inverse dynamics algorithms of Table 2.1 will
produce u + ug. Our objective, however, will generally be to determine solely the
control force vector u.
We now exploit the Principle of Equivalence and initialize the spatial
acceleration _V with a �ctitious nonzero term. Using the gravitational constant of
gc = 9:806 m=s2, this is done by letting
_V0 = [ _wT0 _vT0 ]T = [0 0 0 0 0 gc]
T (2.65)
signify a �ctitious upward, vertical spatial acceleration of the inertial frame. This
technique will allow us to isolate the applied forces u.
36
Representation Inverse Dynamics
Body:
V = �H _��
V = �(H �� � ~V �V )
f = �T�Mi(
�
Vi + _Vg)� ~V T
iMiVi
�u = HTf
Inertial:
V = �H _�
_V = �(H �� � ~V �V ) + ~V
f = �T�M( _V + _Vg)� (M ~ + ~V TM)V
�u = HTf
Spatial:
V = �H _�
_V = �(H �� � ~V �V )
f = �T�M( _V + _Vg)� ~V TMV
�u = HTf
Table 2.3: Inverse Dynamics with Gravity for Di�erent Representations.
Denote _Vp as the spatial accelerations containing the �ctitious pseudo-
acceleration terms at each link while _V are the true spatial accelerations which are
not initialized with _V0. Their di�erence we will call _Vg,
_Vp = _V + _Vg ; (2.66)
and this new quantity re ects the �ctitious accelerations simulating gravity expe-
rienced at each link. Examination of the recursions for _V in Table 2.1 suggests the
following recursion for _Vg:
_Vg;0 = _V0
for i = 1; :::;N
_Vg;i = �i;i�1 _Vg;i�1
end loop
(2.67)
Not that the above holds equally well for�
V as it does for _V .
37
The stacked inverse dynamics for the various Representations including
the corrections for gravity are listed in Table 2.3. The addition of _Vg in the spatial
force expressions is equivalent to having initialized the spatial acceleration recursion
with _V0 as given in (2.65).
Making substitutions for f and _V or�
V in the stacked inverse dynamical
expressions found in Table 2.3, we arrive at the equations of motion as we did in
equations (2.49) where we derived spatial operator decompositions of M and C.
The additional term involving _Vg results in a spatial operator expression for the
vector of gravitational forces,
G = �ug = HT�TM _Vg : (2.68)
The matrix expression for G has an interesting interpretation. One can
see that the applied force due to gravity ug = �G is calculated by multiplying the
spatial inertia (containing the mass properties of the links) with the spatial accel-
eration due to gravity. Then the forces are propagated with the upper triangular
�T operator and then projected onto the links with HT .
It is in a particularly appealing formalism using the Spatial Representa-
tion, for then �i;i�1 is the identity matrix and _Vg;i is constant and equal to _Vg;0.
The gravitational applied force at the ith link would then be equal to
Gi = �ug;i = HT
NXk=i
Mk_Vg;0 : (2.69)
II.G Forward Dynamics
In the forward dynamics problem, the known quantities are the applied
joint forces, u, and the quantities to be computed are the joint accelerations, ��. It
was shown in [23, 78] how using an articulated body inertia model can lead to a
solution of this problem. An articulated body is a chain of links for which all joint
forces compensated by the Coriolis forces and gravitational forces u = u�C�G are
38
assumed to be zero. This, in essence, resembles a \ oppy" serial chain. If at the
kth link, one assumes that the outboard links (k : : :N) form an articulated body,
one may arrive at a decomposition of the inverse dynamics of the following form:
fi = Pi _Vi + zi :
The operator Pi represents the e�ective spatial inertia of the articulated body at
the ith joint, and it can be shown to satisfy the Riccati-like recursion
Pi�1 = Ti;i�1Pi i;i�1 +Mi�1 : (2.70)
The new operator i;i�1 is a transformation operator very similar to �i;i�1, and it
may be constructed in a recursive manner from �i;i�1 and Pi using the following
additional operator de�nitions [38, 78]:
Di = HT
iPiHi ��i = I � �i = I �HiGi
Gi = D�1iHT
iPi i;i�1 = ��i�i;i�1
(2.71)
We can de�ne the stacked articulated body inertia P = diagfPig which will al-
ways be a block diagonal and symmetric matrix. In stacked notation, the spatial
recursion (2.70) can be written as
P = ET PE +M : (2.72)
Similarly, using the stacked formulations, we have
D = HTPH �� = I � � = I �HG
G = D�1HP E = ��E�(2.73)
and, additionally,
K = GE�:
De�ning a \sweep" to be a single recursion along a serial chain in either
direction, we are able to construct a concise, symbolic form for a 3-sweep recursion
of the forward dynamics. The key to this result was the discovery of an alternative
Innovations factorization for the mass matrix M and its inverse [78] which we
reproduce here:
39
Proposition 15
M = [I +K�H]TD[I +K�H] (2.74)
M�1 = [I �K H]D�1[I �K H]T (2.75)
Proof: From the de�nition ofM in (2.49) and using identities (A.13) and (A.24),
M = HT�TM�H
= [I +K�H]THT TM H[I +K�H]
= [I +K�H]TD[I +K�H]
The inverse of the square-root factor of M is obtained with the identity (A.15)
found in the Appendix. The factorization for the inverse of M then follows.
Using this factorization of M�1 in (2.48) with the expression for C found
in (2.49) and for G in (2.68) leads us to the forward dynamics algorithm.
Proposition 16
�� = [I �K H]D�1hu�HT T
�KTu�M(�~V �V + _Vg)� ~V TMV
�i(2.76)
Proof: We use identity (A.13) and some algebraic manipulation in the following
equations to establish the result.
�� = M�1(u� C � G)
= [I �K H]D�1[I �K H]Tu
�[I �K H]D�1HT T�[�M�~V � � ~V TM ]V �M _Vg
�= [I �K H]D�1
hu�HT T
�KTu�M�~V �V �M _Vg � ~V TMV
�i
Remark 17 In this section no mention was made as to whether the Spatial, Iner-
tial, or Body Representations was used. This is because in the forward dynamics
case the algebraic form of the equations is invariant as to the choice of Represen-
tation.
40
II.H Tree-Structured Dynamics and Spatial Recursions
With increasingly complex systems, it becomes necessary to go beyond
modeling multibody systems as single-chain manipulators. The dynamical models
presented so far can be extended to the larger class of systems called tree-structured
systems. The dynamical algorithms will stay essentially the same, only with slight
modi�cations to the recursive operators, E�, E , �, and and additional encom-
passing loop to iterate over the number of branches in the system.
Let Nb be equal to the number of branches in the system, while Nj , j =
1; : : : ; Nb, is equal to the number of links in one branch, and NL =PNb
j=1Nj is equal
to the total number of links in the system. We will continue to index the links of
branch j with i where i = 1; : : : ; Nj. As for the spatial operators, V(j) for example
will indicate the stacked spatial velocity for the jth branch while V(j)i will be the
individual spatial velocity of the ith link of the jth branch.
Another important additional function that will be needed for this class
of systems is the connectivity operator j = (j) which takes as an argument the
branch index j and returns the index of that branch's direct predecessor branch,
the branch immediately preceding it in the tree. Branch k is merely a predecessor
of branch j if it lies on the unique path from the base to the branch in question. If
this is the case, then there exists a p 2 1; : : : ; Nb such that k = p
jwhere p is the
p-times composition of the connectivity operator.
A branch can split into multiple children branches only at its tip. Thus,
the tree must be structured in such a way that this is possible. We now de�ne
the construction of the recursive operators. Let j; k 2 f1; : : : ; Nbg be two branch
indices. The recursive operator E� will now be a Nb�Nb matrix operator with the
6�6 operator E�(j;k) in the (j; k)th position. When j = k, we will denote this simply
as E�(j) and this will be just the previously de�ned E� for a single chain describing
branch j. Otherwise, E�(j;k) describes how the spatial quantities interact between
41
branch j and branch k which only happens when they are adjoining branches.
E�(j;k) =
8>>>>>>>>>>>><>>>>>>>>>>>>:
E�(j) j = k
E�(j;k) =
26666640 � � � 0 �(j)1;(k)Nk
.... . .
...
0 � � � 0
3777775 k = j
0 otherwise
(2.77)
The de�nition of E is identical to E�. The de�nition of � = (I � E�)�1 is as
follows where �j similarly represents just the previously de�ned � for single chain
manipulators referring to branch j.
�(j;k) =
8>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>:
�j j = k
�(j;k) =
2666666664
�(j)1;(k)1 � � � �(j)1;(k)Nk)
�(j)2;(k)1. . .
...... � � �
�(j)Nj;(k)1 � � � �(j)Nj;(k)Nk
37777777759p s.t. k =
p
j
0 otherwise
(2.78)
Note that �(j;k) refers to the (j; k)th entry of the � matrix while �(j)r;(k)s, r 2
f1; : : : ; Njg, s 2 f1; : : : ; Nkg, is the 6 � 6 link-level operator. This latter term is
the product of the link-level operators along the path adjoining the two links, i.e.
�(j)r;(k)s = �(j)r;(j)r�1�(j)r�1;(j)r�2 � � � �(j)r;( j)N j� � ��(k)s+1;(k)s (2.79)
The same structure also holds for the operator. Through multiplication, it can
be shown that all the identities presented in Section A.C continue to be satis�ed.
Recall the forward dynamics with gravity equation (2.76). If we wish to
write down the complete dynamical recursions in its link-level iterative form, then
the �rst step is to make associations of temporary variables with intermediate terms
usually beginning with a � or operator.
42
1st Sweep
8>>>>><>>>>>:
V = �H _�
_Vg = E� _Vg
a = �~V �V
2nd Sweep
8>>>>><>>>>>:
b = M(a+ _Vg) + ~V TMV
c = T (KTu� b)
d = D�1[u�HT c]
(2.80)
3rd Sweep
8><>:e = Hd
�� = d�Ke
Once this is done, it is a simple step to convert the terms in (2.80) using
(A.7)-(A.10) from an expression beginning with � or to one with E� or E . For
example, V = �H _� ) V = E�V +H _�. Taking into account the structure of E�
and E , we do this and write down the link-level recursions in Figure 2.2 as three
sweeps of the tree-structured system.
The recursions in Figure 2.2 are indexed in such a way that if X is a
spatial operator and if � is a recursive operator (also ), then
�(j)1;(j)0X(j)0 =
8><>:�(j)1;(j�1)Nj�1
X(j�1)Nj�1j > 1
0 j = 1
�T(j)Nj;(j)Nj+1X(j)Nj+1 =
8>>><>>>:
NbXk=1
�T(j)Nj;(k)1X(k)1If k=jg j < Nb
0 j = Nb
The function Ig(�)=� is an indicator function which is de�ned as
Ifg(�)=�g =
8><>:
1 g(�) = �
0 g(�) 6= �
To avoid an overload of notation, the (j) branch index is left out of the spatial
operator indices, but it is understood to be there.
43
Figure 2.2: Full Link-Level, Spatial, Dynamical Recursions for Tree-Structured
Systems Including the E�ects of Gravity.
44
1
8>>>>>>>>>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>>>>>>>>>:
for j = 1; :::;Nb
_Vg;0 = �[0 0 0 0 0 gc]T
for i = 1; :::;Nj
Vi = �i;i�1Vi�1 +Hi_�i
_Vg;i = �i;i�1 _Vg;i�1
ai = �i;i�1ai�1 � ~V �
iVi
end loop
end loop
(2.81)
2
8>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>:
for j = Nb; :::;1
for i = N; :::;1
i+1;i = ��i+1�i+1;i
Pi = Ti+1;iPi+1 i+1;i +Mi
Di = HT
iPiHi
Gi = D�1iHT
iPi
��i = I �HiGi
bi = Mi(ai + _Vg;i)� ~V T
iMiVi
ci = �Ti+1;i(��
T
i+1ci+1 +GT
i+1ui+1) + bi
di = D�1i(ui �HT
ici)
end loop
end loop
(2.82)
3
8>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>:
for j = 1; :::;Nb
for i = 1; :::;N
��i = di �Gi�i;i�1ei�1
ei = i;i�1ei�1 +Hidi
end loop
end loop
(2.83)
Chapter III
Contact and Collision Robot
Dynamics
III.A Introduction
The extension of the robot dynamical models discussed so far to include
contact constraints with the surrounding environment and the impulsive forces ex-
perienced at moments of collision is an important one. Interaction of a multibody
system with the environment is such a common aspect of real applications that ef-
fective modeling techniques and calculational tools for these situations are required.
The problem, however, is not a simple one as, in general, contact constraints cre-
ate a di�erential-algebraic system which are known to have considerable numerical
di�culties.
The algorithms presented here provide an e�cient means to recursively
calculate and determine the e�ect of contact and impulsive forces. In the case of col-
lision, we make the assumption of a perfectly inelastic collision.1 These algorithms
do not circumvent, though, the necessity of dealing with a di�erential-algebraic sys-
tem. Later in this section, we will introduce a novel Reduced Dynamics Algorithm
which is able to avoid this unpleasant aspect of contact dynamics. The proposed
1The case of dealing with a mixed elastic and inelastic collision is an extremely hard problem which
remains partially unsolved.
45
46
algorithm essentially allows one to evaluate a reduced set of equations of motion
without having to go through the laborious task of actually constructing them. All
the algorithms are e�cient, recursive, and symbolic, and they are based on the
same modeling structure presented in Chapter II.
III.B Contact Algorithm
The algorithms presented here are based on the work found in [1, 78],
though some of the derivations will be di�erent as well as the notation which con-
tinues to be in line with the dynamical models presented earlier.
We begin with a discussion of contact constraints for tree-structured multi-
body systems. The system may have multiple branches which can come in contact
with the environment. Thus, there may exist multiple contact constraints at any
point in time. The algorithms generalize to the case where intermediate links come
into contact with the environment or with other links on the same branch or another
branch. We omit this development in the interest of brevity.
The equations of motion of a system experiencing contact constraints may
be described as
�� =M�1(u+ JTcfc � C � G) ; (3.1)
where fc represents the forces of constraint. We will later refer, meanwhile, to the
generalized accelerations, ��f , generated from the forward dynamics without contact
constraints as the free accelerations,
��f =M�1(u� C � G) : (3.2)
In equation (3.1) and (3.2),M is the square, positive-de�nite mass-inertia
matrix, C is the vector of Coriolis and centrifugal forces, G is the vector of gravi-
tational forces, and u are the applied forces at the links. Let Nc be the number of
contact points at any point in time and dc;k the number of contact constraints at
47
the jth contact point so that the total number of contact constraints is
dc =NcXk
dc;k : (3.3)
Then Jc of (3.1) is the constraint Jacobian of dimension (6 � dc) � NL, where NL
equals the total number of links in the multibody system. The vector fc is the
stacked constraint force vector of length (6 � dc). The constraint force describes the
force that the environment is exerting back onto the system such that the contact
constraint remains ful�lled.
Assume we have one branch tip contact constraint. If the tip contact
constraint is given holonomically as
c(�) = 0 ; (3.4)
then by taking time derivatives we also obtain
Jc _� = 0 (3.5)
Jc�� + _Jc _� = 0 (3.6)
where Jc = @c=@�. We call these equations the contact constraint equations. The
following Contact Algorithm will �nd the correct accelerations for which the last
contact constraint equation (3.6) is satis�ed. Note that it will be assumed that the
�rst two contact constraints are satis�ed before the algorithm is called.
In order to determine the appropriate contact force which enforces the
contact constraint, we must now do some algebraic manipulation. Multiplying
(3.1) by Jc, substituting for Jc�� using (3.6), and rearranging terms gives us
�JcM�1JT
cfc = JcM
�1(u� C � G) + _Jc _� = Jc��f + _Jc _�
The matrix JcM�1Jc will be generically invertible. We may then solve for an
operator expression for the contact forces fc.
Using the symbolic dynamic operators, it is possible to arrive at an even
more succinct expression for fc which does not require the calculation of the con-
straint Jacobian nor of its derivative. In robotics, the Jacobian J usually refers
48
to the mapping which takes the joint velocities to the tip velocity. It's adjoint
also maps the applied force at the tip to its e�ective applied forces at the various
joints. More generally, however, we can de�ne the Jacobian to perform the above
mappings to any set of points of interest in the multibody system. In particular,
we will de�ne the Jacobian J as that operator which takes the joint velocities _� and
maps them to the unconstrained spatial velocities at the points of contact,
Vc = J _� : (3.7)
The stacked vector of unconstrained spatial velocities at the points of contact Vc
will be of dimension (6 �Nc)� 1.
As the contact constraint equation Jc _� = 0 is equivalent to constrain-
ing certain components of the contact point spatial velocities, we may rewrite this
equation as QVc = 0. Q 2 Rdc�(6�Nc) is a constant matrix which maps the compo-
nents of Vc to a dc-dimensional vector. From the spatial operator decomposition of
V , V = �H _�, a decomposition of the Jacobian J and the constraint Jacobian Jc
may easily be derived. Except for Q, we need only the addition of a new matrix
operator B which acquires the spatial velocity at a point of contact on a link from
the spatial velocity at the joint position on that link. We then have the following
spatial operator decomposition for J and Jc:
J = B�H (3.8)
Jc = QB�H : (3.9)
The matrix B = colfBkg; k = 1; : : : ; Nc; is of dimension (6 � Nc) � (6 �PNb
jNj). If the k
th contact point is on the ith link of the jth branch, then the row
Bk contains all zeros except for a nonzero (6� 6) transformation operator �(c)k;(j)i
in the position corresponding to the ith link and jth branch which transforms the
spatial velocity at the joint position V(j)i to that at the kth contact point Vc;k.
Bk =
�0 � � � 0 �(c)k;(j)i 0 � � � 0
�(3.10)
49
It is common that the contact points will be at a branch tip. In this case we may
write Bk as
Bk =
�0 � � � 0 �(j)tip;(j)Nj
0 � � � 0
�: (3.11)
The index (j)tip refers to the point located at the tip of the jth branch.
The matrix Q = diagfQkg, k = 1; : : : ; Nc, has a number of rows equal to
the number of constraints dc though the number of diagonal blocks Qk is equal to
the number of contact points. The di�erence lies in that there may exist more than
one contact constraint at a point of contact. The block Qk 2 Rdc;k�6 singles out
the components of Vc;k which are required to be zero. For example, if at contact of
a branch tip with the ground there is no slipping, but there is rotational freedom,
then
Qk =
26666640 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
3777775 (3.12)
Here, Qk is seen to take only the linear velocities of Vc;k but not the angular veloc-
ities. These result in three contact constraints.
Given the above de�nitions, the next step is to recognize that the time
derivative of QVc = QB�H _� = Jc _� is
Q _Vc = Jc��f + _Jc _� ; (3.13)
so that we may conclude with the following result:
Proposition 18 A recursive, symbolic expression for the contact forces is
fc = ��Q _Vc ; (3.14)
where � = (JcM�1JT
c)�1 is of dimension (dc � dc), _Vc 2 R
6�dc is a stacked spatial
acceleration vector containing the spatial accelerations at all the contact points, and
Q is a constant matrix which singles out the components of _Vc corresponding to the
constrained components of V .
50
The term ��1 = (JcM�1JTc) called the operational space inertia is essen-
tially the mass-inertia matrix for all the constraint points in the system [48, 51].
We will later discuss how to calculate this term as it can be somewhat involved for
tree-structured systems.
We will now make some comments regarding the calculation of _Vc. Recall
that _V and, consequently, _Vc will have di�erent values depending on the choice of
Representation. What is of interest, however, is the actual physical accelerations
occuring at the points of contact. This prevents the use of the Spatial Represen-
tation for this Contact Algorithm as the Spatial Representation gives the spatial
acceleration with respect to a point coincident with the origin but moving as if �xed
to the body. Also of interest are the total time derivatives, not the local spatial
accelerations�
V. It is still possible to work with the Body Representation, we just
need to remember to use _V . The expression for _V from the Inertial Rpresentation
_V = �(�~V �V +H ��) + ~V (3.15)
will also work for the Body Representation, only now _V will not refer to the total
time derivative of V with the state vector represented in body coordinates. To
obtain the total time derivative, it is necessary to add a crossterm correction vector
d
dtV = _V + ~V : (3.16)
This latter step, however, is not required for the Contact Algorithm as it is assumed
that Vc = 0.
The correct transformation rule to acquire the tip acceleration _Vtip has also
not yet been discussed which will be needed in the event that a contact constraint
occurs at a branch tip. For example on branch j, determine _V(j)tip from the spatial
acceleration at the joint position _V(j)Nj. By taking derivatives of the spatial velocity
propogation rule (2.5), we obtain
V(j)tip = �(j)tip;(j)NjV(j)Nj
(3.17)
_V(j)tip = �(j)tip;(j)Nj( _V(j)Nj
� ~(j)NjV(j)Nj
) + ~(j)tipV(j)tip (3.18)
51
Finally, to conclude the Contact Algorithm, we must �rst acquire the
corrected generalized accelerations. From the dynamics (3.1), we recognize that
�� = ��f +M�1JT
cfc = ��f + ��� ; (3.19)
where we have de�ned ��� as the corrective accelerations, which are to be added to
the free accelerations calculated from the forward dynamics algorithm. In summary:
Contact Algorithm
1. Calculate ��f from the forward dynamics.
2. Calculate _Vc using the inverse dynamics algorithm and, if necessary, the ap-
propriate propogation rules such as (3.18) for branch tip contact.
3. Calculate fc from (3.14).
4. Calculate ��� =M�1JTcfc.
5. Add ��� to ��f to obtain the �nal accelerations.
III.C Collision Algorithm
A very similar algorithm exists for calculating the change in joint velocities
due to an inelastic collision with the ground. We assume that at the moment of
collision with the ground, the system experiences an instantaneous impulsive force
fimp. This results in a sudden change in the generalized velocities. The amount of
change will depend on the tip velocities at the moment of contact with the ground.
Another consequence of a discontinuous jump in the velocities is also a sudden drop
in the kinetic energy of the system.
The following derivation is borrowed largely from [1]. As a shorthand
notation, let u = u� C � G contain all the terms in the equations of motion (3.1)
which do not contain any accelerations nor any contact forces. Then integrating
52
these equations of motion over an in�nitesimal amount of time gives
lim�t!0
Zt+�t
t
M�� dt = lim�t!0
Zt+�t
t
JTcfc + u dt : (3.20)
This equation then implies
M� _� = JTcfimp where fimp = lim
�t!0
Zt+�t
t
fc dt : (3.21)
The term � _� refers to the change in joint velocities from immediately before to
immediately after the collision, and it is this term that we would like to calculate,
i.e. � _� = _�+ � _��.
Let �Vc = V +c� V �
cbe similarly the change in spatial velocities at the
points of contact from before and after the collision. Multiplying the expression for
� _� in equation (3.21) by JcM�1 and recognizing that Q�Vc = Jc� _� results in the
expression (JcM�1JTc)fimp = Q�Vc. From this we conclude:
Proposition 19
fimp = �Q�Vc (3.22)
where � = (JcM�1JTc)�1.
Note that, in general, a zero tip velocity is desired in the constraint directions. In
this case, QV +c= 0 and fimp = ��Q�V �
c.
Equation (3.21) now implies � _� =M�1JTcfimp. The joint velocities after
collision are then computed by _�+ = _�� +� _�. In summary:
Collision Algorithm
1. Calculate Vc from the calculated spatial velocities V using the propogation
rules such as (3.17) for branch tip collisions.
2. Calculate fimp from (3.22).
3. Calculate � _� =M�1JTcfimp.
4. Add � _� to _�� to obtain the joint velocities after collision.
53
III.D Calculation of ��1 in the Contact and Collision Al-
gorithms
We have shown that the term � = (JcM�1JTc)�1 is required to compute
fc and fimp. This term is a di�cult one to calculate as we must construct the
entire matrix ��1 in order to invert it. This matrix of dimension dc � dc has a size
depending on the number of contact and collision cases. In the collision case, it is
with all likelihood that only one branch tip collides with the environment at any
one instant in time. However, to calculate the change in velocities, we must include
all the contact constraints into Jc, including the new collision case, such that all
the resulting velocities will satisfy equation (3.5) for the upcoming contact phase.
Though no closed form operator expression presently exists for �, we can
make use of the spatial operator identities to some extent to produce an O(N2)
algorithm. We may rewrite ��1 with the help of Identity (A.13) as
��1 = JcM�1JT
c
= QB�H[I �H K]D�1[I �H K]THT�TBTQT
= QB HD�1HT TBTQT (3.23)
Let X = HD�1HT and suppose there exists a matrix operator S which satis�es
the recursion
X = S � E SET
: (3.24)
Using Identity (A.10), we can then de�ne another operator Y which satis�es the
matrix recursion:
Y = X T = HD�1HT T (3.25)
= S + � S + S � : (3.26)
In the case of a single-chain manipulator, S is block diagonal and it is straightfor-
ward to calculate S using the recursion (3.24). For general tree-structured systems,
S will have o�-diagonal blocks as well which complicate its computation. Let S(j;k)
54
be the (6 � Nj) � (6 � Nk) block component of S corresponding to branch j and k
while S(j) refers to the diagonal block component of S. Then S has the following
structure:
S(j) = E (j; j)S( j)ET
(j; j)+ E (j)S(j)E
T
(j) +HjD�1jHT
j
S(j;k) =
8>>>>>>>><>>>>>>>>:
E(j; j)S( j ; k)ET
(k; k)j > k; 9 p; q > 0;
+E (j)S(j;k)ET
(k;k) s.t. p
j=
q
k
ST(k;j) j < k
0 otherwise
(3.27)
In the interest of brevity, we will restrict the upcoming analysis to only
branch tip contacts and collisions. In this case, we need only consider the compo-
nents of Y which reference the last link of each branch. These components we will
reference with only the branch index, i.e. Y(j;k) refers to the interaction between
the last link of branch j and of branch k. It has the following structure:
Y(j;k) =
8>>>>>>>><>>>>>>>>:
S(j)Njj = k
(j)Nj;(k)NkS(k)Nk
j > k; 9 p s.t. pj= k
(j)Nj;(m)NmS(m)Nm T
(k)Nk;(m)Nmj > k; 9 p; q s.t. m =
p
j=
q
k
Y T
(k;j) j < k
(3.28)
Let Z = BYBT and �r be the branch number which the rth tip contact
point refers to. Then Z = fZ(r;s)g, r; s 2 f1; : : : ; Ncg is also a symmetrical matrix
composed in the following way:
Z(r;s) = �(�r)tip;(�r)N�rY(�r;�s)�
T
(�s)tip;(�s)N�s(3.29)
We present in Figure 3.1 a recursive algorithm for calculating ��1. Recall that pj
is the pth predecessor branch of branch j and dc is the total number of contact and
collision constraints.
55
Figure 3.1: Recursive algorithm for calculating ��1 in the case of branch tip contact
or collision.
56
8>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>><>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>:
for j = 1; :::;Nb
if j > 1; j; j= (j)1;( j)N j8>>>>>>>>>>>><
>>>>>>>>>>>>:
for i = 1; :::;Nj
S(j)i = (j)i;(j)i�1S(j)i�1 T
(j)i;(j)i�1 +H(j)iD�1(j)iH
T
(j)i
if j > 1;
j; j= (j)i;(j)i�1j; j
end loop
Yj;j = S(j)Nj
if j > 1;
Yj; j = j; jS( j)N j8>>>>>>>>>>>>>>>>>>><
>>>>>>>>>>>>>>>>>>>:
for k = j� 1; :::;1
if (9 p > 1 s.t. pj= k);
j;k = j;
p�1j
p�1j
;k
else,
j;k = 0
Yj;k = Y T
k;j= j;kS(k)Nk
end loop8>>>>>>>><>>>>>>>>:
for k = 2; :::; j� 1
if (6 9 p > 0 s.t. pj= k);
Yj;k = Y T
k;j= Yj; k
T
k; k
end loop
end loop8>>>>>>>>>>>><>>>>>>>>>>>>:
for r = 1; :::;dc8>>>>><>>>>>:
for s = 1; :::;dc
��1r;s
= ��Ts;r
= Q�r�(�r)tip;(�r)N�rY�r;�s�
T
(�s)tip;(�s)N�(s)QT
�s
end loop
end loop
57
III.E Introduction of External Forces
It is sometimes desirable to introduce external forces to the system in
addition to those applied at the joints. For example, as will be seen in the case
study of the biped walker in Chapter VII, we add to our model an ankle torque at
the point of contact of the leg with the ground even though no foot is explicitly
modeled. This allows a reduction in the dimensionality of the problem by having
one less link in the multibody system.
The introduction of a force at the tip enters into the equations of motion in
the same manner as a contact force does in equation (3.1). Let fe be the external
force and Je be the Jacobian whose transpose transforms fe into the resulting
additional applied forces experienced at the various joints. Then using Identity
A.13,
�� = M�1(u+ J ecfc � C � G) (3.30)
= [I �K H]D�1hu�HT T
�KTu�M(�~V �V � _Vg)� ~V TMV
�i+[I �K H]D�1HT TBTfe
= [I �K H]D�1hu�HT T
�KTu�M(�~V �V � _Vg)� ~V TMV
�BTfe�i
(3.31)
It may also be of interest to introduce an arbitrary impulsive force to the
system. This also follows as in the collision case and requires only substituting
fimp into _�� = M�1JTefimp to determine the corrections to be added to the joint
velocities.
III.F Reduced Dynamics Algorithm
With the introduction of homonomic constraints, such as contact con-
straints with the environment, we create a so-called differential-algebraic system.
The system acquires a new set of dynamical equations which describe its motion in
58
a now smaller dimensional space which is commonly called the constraint manifold.
In general, however, we will know the dynamical equations only for the free system
plus the additional algebraic contact constraints which describe the shape of the
constraint manifold. It is then necessary to use the contact constraints in combina-
tion with the free dynamical equations to determine the motion of the constrained
system on the constraint manifold.
The Contact Algorithm discussed in Section III.B determines the corrected
accelerations which satisfy the acceleration contact constraint equation (3.6). The
resulting di�erential-algebraic system can cause a su�cient amount of numerical
di�culties during integration of the dynamics. There exist various solutions to
integrating di�erential-algebraic systems which are surveyed and discussed in the
references [3, 73, 86].
A better alternative, however, to using specialized integration software is
to be able to construct the reduced, unconstrained set of dynamics which evolves
directly on the constraint manifold. As these equations can often be di�cult to
determine, they are not used very frequently. In the case of multibody systems,
though, there exist many cases for which they can be determined. For these cases,
we present here an algorithm which evaluates these reduced dynamics without hav-
ing to undergo the laborious task of actually constructing them. With only a
minimum of additional calculation, we may use just slight variations of the pre-
viously discussed algorithms to calculate the reduced accelerations of the reduced
dynamics which may be integrated free of numerical problems, after which we can
reconstruct the complete state from the updated reduced state.
In order to satisfy the velocity constraint condition (3.5), the generalized
velocities _� must belong to the null space of the constraint Jacobian, N (Jc) �
RN�m. If the columns of X represents a basis for N (Jc), then there exists a
representation of _� with respect to X denoted here by �, _� = X�. Substituting
�� = X _�+ _X� into the dynamical equations (3.1) and multiplying on the left by XT
59
will give us the reduced dynamics,
M�_� + C� + G� = u� ; (3.32)
where M� = XTMX, C� = XTM _X� +XTC, G� = XTG, and u� = XTu.
If � represents the generalized coordinates of the system, then it is possible
to choose N �m locally independent coordinates �1 and m dependent coordinates
�2 such that Jc;1 _�1 + Jc;2 _�2 = 0 may be used as an alternative expression for (3.5).
This approach was made in [85], and it leads to an obvious choice for X,
_� = X� = X0_�1 =
264 I
�J�1c;2 Jc;1
375 _�1 : (3.33)
Remark 20 An advantage of making this choice for the basis X is that, as will
be shown in the Reduced Dynamics Algorithm, the reduced accelerations are simply
represented as _� = ��1.
Our goal here is to show that the solution of the contact algorithm may
be used to obtain a solution of the reduced forward dynamics problem. Then an
optimization routine performing numerical integration need only integrate on the
independent coordinates ��1. We �rst give a lemmabefore the algorithm is presented.
Lemma 21 Let X be a basis for the null space of the constraint Jacobian, N (Jc),
and assume that at time t = 0, the state (�; _�) satis�es the constraint conditions
c(�(0)) = 0, Jc _�(0) = 0. Then the following statements are equivalent:
(a) _� and �� satisfy Jc�� + _Jc _� = 0.
(b) There exists an N �m dimensional vector � which satis�es _� = X�.
(c) X _� = �� � _X� is consistent and has a unique solution _� = fracddt�.
Proof: (c)) (a) Since X is a basis for N (Jc), then JcX� = 0 and d=dt(JcX�) =
_JcX� + Jc _X� + JcX _� = 0. So,
Jc�� + _Jc _� = JcX _� + (Jc _X + _JcX)� = 0 :
60
(a) ) (b) Integrating (a) implies Jc _� = 0 since Jc _�(0) = 0 at time t = 0. Jc _� = 0
further implies that there exists a representation � for _� with respect to X, _� = X�.
(b)) (c) Di�erentiating _� = X� and observing that X is full rank gives the desired
result.
If we substitute the calculated accelerations �� from the Contact Algorithm
into (a), we see that with some algebraic manipulation, this expression will be
satis�ed.
Jc�� = JcM�1(u� C � G) + JcM
�1JTcfc
= JcM�1(u� C � G)���1�Q _Vtip
= JcM�1(u� C � G)� Jc��f � _Jc _�
= � _Jc _� (3.34)
The importance of this fact is that it allows for the possibility to use the solution
of the Contact Algorithm for evaluating the reduced state accelerations.
Reduced Dynamics Algorithm
1. Beginning with an independent set of coordinates � = �1, solve via inverse
kinematics for �2 from �1. Similarly solve for _�2 from _�1 using the algebraic
relation _�2 = �J�1c;2Jc;1 _�1.
2. Given a set of torque inputs u, one may solve for �� with the Contact Algo-
rithm. Equation (3.34) shows how the solution satis�es (a) of Lemma 21.
3. Using Eq. (3.33), it follows that _� = ��1, and it satis�es the reduced dynamics
(3.32).
This algorithm thus yields the reduced forward dynamics mapping u !
(�; _�). The limitation of this approach is the requirement to solve an inverse kine-
matics problem in Step 1 which may be di�cult. In some cases, a simple closed-form
61
solution may exist, and if there exists a multiple number of solutions, then the pre-
vious con�guration can be used to determine the correct updated con�guration.
More generally, a closed-form solution will not exist, then the standard approach
is to solve this nonlinear zero-�nding problem with iterative techniques for which
there exist many possibilities. See for example [24].
The calculation of the velocities _�2 = �J�1c;2Jc;1
_�1 is easier. It is always
possible to construct the matrix operators Jc;1 and Jc;2 using its symbolic matrix
decomposition. Moreover, it is also possible to use a variation of the Collision
Algorithm which only solves for the dependent state velocities such that the system
satis�es the constraint condition (3.5). In the case study to be presented later on
biped locomotion, we are able to use the above algorithm together with a closed-
form solution for the inverse kinematics problem. We also use the technique of
calculating the dependent velocities with the Collision Algorithm.
III.G Closed-Chain Dynamics
The development presented in this chapter can also be extended to the case
of closed-chain systems which includes those systems for which a branch may close
in on itself or on another branch of the multibody system. The algorithms for this
more general case were omitted as they were not necessary for the applications of
interest, namely the biped walking problem to be presented later. Roughly though,
the process for constructing the dynamics of such systems is to �nd a spanning
tree of the multibody system and then break the closed-chains to obtain a tree-
structured system. It is then possible to compute the forward dynamics and, in the
process of reattaching the chains, the Contact Algorithm can be used to �nd the
�nal accelerations for the system. For more details, see [77].
Chapter IV
Derivatives of Spatial Dynamic
Operators and Applications
IV.A Introduction
Already we have used closed form, symbolic expressions for the derivatives
of spatial operators to construct the inverse and forward dynamics. These deriva-
tives have enabled us to develop powerful and exible, recursive, symbolic equations
for the evaluation and manipulation of the equations of motion. Further applica-
tions of these derivatives were presented in Chapter III for the cases of contact and
collision of a multibody system with the environment. There exist many further
applications which can make use of derivative expressions of the dynamics in areas
ranging from control to video motion estimation. The advantages are being able
to derive an expression which gives insight into its own structure and calculation.
Also, many simpli�cations can sometimes be made using the many identities that
exist.
In this chapter, we complete the results on derivatives for the most com-
mon operators and summarize them for the various Representations. We then
present several applications of their uses. A Lagrangian derivation of the dynam-
ics is presented which di�ers from the \bottom-up" approach taken in Chapter II.
62
63
Derivatives of the energy in the system are presented. Then, in the special case
of the Spatial Representation, we present, making several extensions, the partial
derivatives of the dynamics with respect to the state variables. The ability to ob-
tain closed-form recursive expressions for these quantities which may then be used
in many control and estimation applications is extremely valuable and has not been
su�ciently recognized in the research community.
IV.B Derivatives of Spatial Operators
In Table 4.1 are listed the derivatives of several operators including a local
derivative under the Body Representation and a total derivative using the Inertial
and Spatial Representations. The local time derivative is an interesting quantity as
it is a natural derivative to be used when the dynamics are calculated in coordinates
with respect to a frame �xed in the body. This derivative, which we denote as a
circle derivative, was �rst introduced in Section II.A.2 and then later in Section
II.C. The �rst four lines of the table all list derivatives which have been presented
in the inverse dynamics sections of Chapter II. The remaining derivatives are of
those new matrix operators introduced in the forward dynamics section, Section
II.G.
Jain derives the derivatives for the Body and Inertial cases in the single
degree-of-freedom case [44], while we generalize these results and add the derivatives
for the Spatial case. The spatial time derivatives of E� and � are easily seen to be
zero as these are constant matrices in the Spatial Representation (see Eq. 2.62).
The derivative of H was derived with equation (2.58). The remainder of this section
will be used to prove the derivative results which have not yet been discussed in
this work beginning with the local derivative expressions.
Proof of Body Local Time Derivatives:
The local derivatives of E� and � are taken from (2.53) and (2.54) respectively
while those of H and M follow from their construction. An important operator in
64
OperatorBody Local
Time Derivative
Inertial Total
Time Derivative
Spatial Total
Time Derivative
E��
E� = �~V �E��
E� +~E� � E� ~ 0
��
� = ��~V �E���
�+~�� �~ 0
H 0 ~H ~V H
M 0 ~M �M ~ �~V TM �M ~V
D �D = HT�PH �D �D
G �G = D�1HT�P �� �G �G~ �G �G~V
���
��= �H�G�
�� +~�� � �� ~�
�� +~V �� � �� ~V
E �HD�1HT�P E + ���
E��
E +~E � E ~�
E +~V E � E ~V
P�P = ET
�(��T�P �� � ~V �TP ��
���TP ~V �)E�
�P + ~P � P ~ �P � ~V TP � P ~V
Table 4.1: Time Derivatives of Spatial Operators
Table 4.1 is the local and total time derivative of P . The derivatives of the forward
dynamics matrix operators all depend on the derivative of this quantity, yet because
of the recursive nature of P , it is best to derive the other matrix operators �rst in
terms of �P . Lastly, we will derive the recursion for �P .
The body derivative of D = HTPH is immediate from previous de�nitions.
For the body derivative of G = PHD�1:
�G = �D�1�DD�1HTP +D�1H�P
= D�1HT�P [I �HG]
= D�1HT�P ��
The body derivative of �� = [I �HG] is immediate from previous de�nitions.
For the body derivative of E = ��E�:
�
E = �H�G E� + ���
E�
= �HD�1HT�P ��E� + ���
E�
65
= �HD�1HT�P E + ���
E�
In order to derive a recursion for �P , it is easier to take the circle derivative of the
simpler P recursion found in Identity (A.22).
0 = �M =�
ET�PE � E
T
��P E � E
T
�P
�
�� E� � ET
�P ��
�
E�
= �P � ET��P ��E� + E
T
�PHD�1HT�P ��E��
�
ET�P ��E� � E
T
�P ��
�
E�
= �P � ET���T�P ��E��
�
ET�P ��E� � E
T
���TP
�
E�
= �P � ET�(��T�P �� � ~V �TP �� � ��TP ~V �)E�
The term enclosed by ET�and E� indicates a backwards recursion which produces a
block diagonal matrix. Thus, it is seen that �P will only depend on the other circle
derivative expressions from previous iterations.
Proof of Inertial Time Derivatives:
The inertial time derivatives of E�, H, and M are merely stacked versions of (2.35),
(2.36), and (2.14). From Identity (A.7), di�erentiating �(I � E�) = I results in
_� = � _E��. Further use of Identity (A.9) gives the result. The inertial time derivative
of P will initially be assumed to be of the same form as that of M , namely
_P = �P + ~P � P ~ : (4.1)
We will then use this expression in the derivation of the other forward dynamics
terms much as we did in the body local derivatives proofs.
_D = _HTPH +HT _PH +HTP _H
= HT ~TPH +HT (�P + ~P � P ~)H +HTP ~H
= HT�PH �HT ~PH +HT ~PH
= HT�PH
In the last equation, we used the fact that ~T = �~ as it is block diagonal matrix
consisting of skew-symmetric blocks.
_G = �D�1 _DD�1HTP +D�1 _HTP +D�1HT _P
66
= �D�1HT�PG+D�1HT ~TP +D�1HT (�P + ~P � P ~)
= D�1HT�P [I �HG]�G~
= D�1HT�P �� �G~
_�� = �~HG�H(�G �G~)
=�
�� +~[I �HG] � [I �HG]~
=�
�� +~�� � �� ~
_E = (�
�� +~�� � �� ~)E� + �� (�
E� +~E� � E�~)
=�
�� E� + ���
E� +~E � E ~� �� ~E� + �� ~E�
=�
E +~E � E ~
Now we will see that, in fact, the assumed form of _P does indeed satisfy the time
derivative of the P recursive Riccati equation, (A.22). In this way, it is shown that
(4.1) is a valid closed form expression for _P . With the help of identity (A.21) and
the expression obtained for �P , we take the derivative of (A.22),
_M = _P � _E�T
PE � ET
�_PE � E
T
�P _E
= (�P + ~P � P ~)� (�ET�~V �T + ET
�~T � ~TET
�)PE
�ET�(�P + ~P � P ~)E � E
T
�P (
�
�� E� + ���
E� +~E � E ~)
= �P + ~P � P ~ + ET�~V �TP ��E� � ~ET
�PE � E
T
��P E
+ET�~PE � E
T
�
~PE + ET
�P ~E � E
T
�P ~E
�ET�P (�HD�1HT�P ��E� � �� ~V �E�) + E
T
�PE ~
= �P + ~(P � ET�PE )� (P � ET
�PE )~� E
T
��P E + E
T
��P ��E�
�ET�[I �GTHT ]�P ��E� + E
T
�( ~V �TP �� + ��TP ~V �)E�
= ~M �M ~ +�P � ET�(��T�P �� � ~V �TP �� � ��TP ~V �)E�
= ~M �M ~
We, thus, arrive at the already estabished derivative of M proving that (4.1) is a
valid expression for _P .
67
Proof of Spatial Time Derivatives:
The time derivatives of E� and � follow from their construction presented in equation
(2.62) while those of H and M come from equations (2.58) and (2.28). The Spatial
time derivatives follow in much the same way as in the proofs of the Inertial time
derivatives with only minor exceptions. The form for _P will similarly be initially
assumed from the form of _M , namely
_P = �P � ~V TP � P ~V : (4.2)
In the case of _E , we will need the use of an additional identity, (A.25).
_E = (�
�� +~V �� � �� ~V )E�
=�
�� E� + ~V E� � ��(E� ~V + ~V �E�)
=�
�� E� + ���
E� +~V E � E ~V
=�
E +~V E � E ~V
It can also similarly be shown, as in the Inertial case and with the additional use of
identities (A.25) and (A.26), that the expression for _P from (4.2) satis�es the time
derivative of the recursive Riccati equation (A.22).
IV.C A Lagrangian Derivation of the Dynamics
In Section II.B, stacked spatially recursive operator expressions were given
for the mass matrixM and the Coriolis vector C. These were obtained by stacking
the operators used for the dynamics calculation at the link level, then substituting
for the various equations to obtain a single equation which described the equations
of the system. In section II.F, we then discussed the e�ect of a uniform gravitational
�eld, G, and we derived a spatial operator decomposition for it. We have thus
obtained a complete dynamical decomposition into spatial matrix operators for the
68
entire equations of motion.
M(�)�� + C(�; _�) + G(�) = u : (4.3)
The equations of motion that we have developed thus far have been con-
structed from physical properties on the link level and the interlink relationships.
A compact dynamical description for the entire system was obtained by stacking
the link level spatial matrix operators into larger, more succinct matrix expressions.
For mechanical systems, it is common to obtain dynamical expressions for a system
by using the well-known Euler-Lagrange equation 1,
d
dt
@L
@ _��@L
@�= uT ; (4.4)
where L = KE � PE is called the Lagrangian.
In this section, we show how it is also possible to derive the equations
in this manner assuming expressions for the kinetic energy and potential energy,
and the derivatives of the matrix operators presented in Section IV.B. We restrict
our attention to the special case when the links of the multibody system may be
modeled as a succession of single degree-of-freedom joints. A su�cient condition for
this possibility to occur is that each link have at most one translational degree of
freedom with no restrictions on the rotational degrees of freedom. This restriction
is necessary for the derivation of the mass-inertia sensitivity matrix to be used later.
In Section II.A.1, a quadratic form for the kinetic energy was presented.
The physical principle that the kinetic energy is a constant independent of any
reference frame location on a rigid body immediately implied a form for the spatial
inertia Mx at any point x. By further accepting the construction of the spatial
velocity V = �H _� and its interaction withM to make up the kinetic energy for the
system, one arrives at the Newton-Euler decomposition for the mass-inertia matrix
M.
KE =1
2
NLXi
V T
iMiVi =
1
2V TMV
1 @
@�is the row operator, @
@�= ( @
@�1� � �
@
@�N)
69
=1
2_�THT�TM�H _� (4.5)
=1
2_�TM _�
For obtaining the decomposition of M, we only needed to know the relationship
that existed between the spatial velocities V and the angular velocities _�. This
relationship was discussed in Section II.A.1.
Similarly, the potential energy for a uniform gravitational �eld may also
be written down as a sum of quadratic forms derived from physical relationships of
each link. The well-known physical principle mass�gravity�height may be written
as
PE =NLXi
mi gc hC;i (4.6)
where mi is the mass of link i, gc is the gravitational acceleration constant, and
hC;i is the vertical height of the center of mass of link i relative to the origin.
We have already constructed a matrix decomposition (Newton-Euler) for
the mass-inertia matrix, M. Consequently, the Innovations factorization (2.74)
used in Section II.G for the forward dynamics, may be constructed with matrix
identities. What remains is a derivation for the vector of Coriolis and centrifugal
forces C(�; _�) and the vector of gravitational forces G(�). These can be derived from
the Euler-Lagrange equation (4.4).
Substituting for the Lagrangian function L in the Euler-Lagrange equation
the kinetic energy minus the potential energy, L = KE � PE , and taking the
transpose, we arrive at the following expression:
M�� + _M _� �1
2_�TMT
�
_� + (PE )T�= u : (4.7)
The shorthand notation (PE )�is for the partial derivative with respect to � of the
potential energy while _�TMT
�_� represents col [ _�TM�i;k
_�] whereM�i;kis equal to the
partial derivative of M with respect or the kth component of the ith joint position
vector.
70
Associating (4.7) with the general form for the equations of motion of
mechanical systems (4.3), we arrive at G = r�(PE) = (PE)T�and the following
well-known expression for C(�; _�) [57],
C(�; _�) = _M _� �1
2_�TMT
�_� : (4.8)
We now require an operator expression for _M, the sensitivitiesM�i;k, and (PE)�.
To obtain _M one may use the derivatives of the spatial operators from
Table 4.1 to di�erentiate the Newton-Euler factorization of M. Interestingly, the
result will be the same regardless of the choice of Representation and derivative.
Thus, using a local derivative with the Body Representation or an inertial time
derivative with any of the Representations leads us to the following expression for
_M =�
M.
Proposition 22
_M =�
M= HT�Th�ET
�~V �T�TM �M�~V �E�
i�H (4.9)
Proof: We present here a derivation using the Spatial Representation, though
the simplest derivation is certainly using the local circle time derivatives. Recall
that the time derivative of � is 0 in this case. The �nal result employs the use of
identity (A.26).
_M = _HT�TM�H +HT� _M�H +HT�TM� _H
= HT ~V T�TM�H +HT�T (�~V TM �M ~V )�H +HT�TM�~V H
= HT ( ~V T�T � �T ~V T )M�H +HT�TM(�~V �+ �~V )H
= HT�Th�ET
�~V �T�TM �M�~V �E�
i�H
Jain and Rodriguez use the derivatives to obtain a coordinate independent
representation for the sensitivities of the mass matrix M�iin [44]. We reproduce
this result here which is valid for single degree-of-freedom joints.
71
De�ne the new stacked matrix operator ~H(i) which contains all zeros ex-
cept in the link i block position on the diagonal where there is found ~Hi. Then we
have the following result.
Proposition 23
M�i= HT�T
h�E� ~H
(i)T�TM �M� ~H(i)E�i�H (4.10)
_�TMT
�_� = 2HT�T
h~V T � ~V �T�T
iMV (4.11)
Proof: Using identity (A.31), we have that @M
@�i= @ _M
@ _�i. Recall that the stacked
spatial operator ~V � has on its diagonalgHi
_� The partial derivative of ~V � with respect
to _�i is thus ~H(i). Since this is the only term in _M which contains _�, the result for
M�ifollows from (4.9).
As previously de�ned, _�TMT
�
_� = colf _�TMT
�i
_�g. From identity (A.9), we
rewriteM�ias
M�i= (HT�T �HT )
h� ~H(i)T�TM �M� ~H(i)
i(�H �H) : (4.12)
Then, pre-and post-multiplying by _� gives
_�TMT
�_� = 2colf�(V T � V �T ) ~H(i;k)T�TMV g
= 2diagfHT
i( ~V T
i� ~V �T
i)g�TMV
= 2HT ( ~V T � ~V �T )�TMV
= 2HT�T ( ~V T � ~V �T�T )MV
Since _�TMT
�i
_� is a scalar quantity, we are able to combine the transposed quantities
in the �rst equation. The �nal step comes from using identity (A.26).
A useful result for systems with single dof joints is the identity
Identity 24
~V �H = 0 : (4.13)
72
Proof: Both spatial operators are block diagonal so that their product consists
of ~V �
iHi =
gHi
_�iHi = � ~HiHi_�i = 0.
Using the previous results, we may now derive the equations for the Cori-
olis forces from the Lagrange equations via (4.8). First of all, identities (4.13) and
(A.9) give an alternative expression for _M.
_M = HT�Th�~V �T�TM �M�~V �
i�H (4.14)
C = _M _� �1
2_�TMT
�_�
= HT�Th�~V �T�TM �M�~V �
i�H _� �HT�T
h�~V �T�T + ~V T
iM�H _�
= HT�Th�M�~V � � ~V TM
i�H _� (4.15)
Note that the expression derived for C(�; _�) is the same as found in (2.49) when the
equations of motion are developed from the bottom up.
Finally, we seek to arrive independently at the known expression for the
gravity term G = (PE )T�presented in equation (2.68) of Section II.F. Beginning
with the potential term PE (4.6), we can use identity (A.31) of the Appendix.
According to identity (A.31), (PE )� = _(PE ) _� since (PE ) _� = 0. From (4.6),
_(PE ) =NLXi
gcmi vz;C;i (4.16)
The scalar vz;C;i is the z-component of the linear velocity of the center of mass of
link i. We continue with the Spatial Representation, for it is with this choice of Rep-
resentation that _Vg;0 is constant and equal to the vector _Vg;0 = [0 0 0 0 0 gc]T
for all links. Then,
_(PE ) =NLXi
_V T
g;iMC;iVC;i
=NLXi
_V T
g;i�TC;i
�1�TC;iMC;i�C;i�
�1C;iVC;i
73
=NLXi
_V T
g;i�Ti;CMi�i;CVC;i
=NLXi
_V T
g;iMiVi
= _V T
gMV (4.17)
Note that the rigid body transformation operator �i;C, which transforms a spatial
vector from its de�nition at the center of mass to that at the joint position of link
i, is of the form such that
�i;C _Vg;i =
264 I 0
�~lC;i I
375 _Vg;i = _Vg;i :
Finally, it is easily seen that the only term dependent on _� in _(PE ) is
V = �H _� so that (PE ) _� =_V T
g;iM�H or
G = (PE )T�= HT�TM _Vg (4.18)
A change of representation to either the Inertial or the Body Representa-
tion may be made via the set of rigid body transformation operators E� in stacked
form. Then it can be shown how E� will be absorbed into the matrix operators found
in (4.18) proving that (4.18) is in fact a coordinate independent representation.
Beginning with the Newton-Euler decomposition of M and using the
derivatives of the spatial operators in Section IV.B, we have derived expressions
for the Coriolis terms and gravitational forces in the equations of motion in a top-
down fashion. This direct approach is in contrast to all previous approaches made
to derive symbolic recursive dynamics such those found in [15, 70, 72, 78].
IV.D Derivation of Coriolis Matrix for Skew-Symmetry
Property
Often it is necessary to factor a square matrix C which multiplies _� to
form C. Though this may be straightforward from the expression given for C, there
74
may exist more than one form for C. In control design applications, for example, a
well-known and often used result is that _M�2C can be taken to be skew-symmetric
[57]. Using the expression for C given in (4.15), we have
_M� 2C = HT�Th�~V �T�TM +M�~V � + 2~V TM
i�H : (4.19)
Note that the �rst two terms make up together a skew-symmetric quantity as the
transpose of �~V �T�TM+M�~V � is the negative of itself where the symmetric nature
of M is exploited. The remaining term ~V TM is not skew-symmetric as this does
not equal �M ~V in general. This is a consequence of the fact that the choice for C
is not unique. If we exploit the fact that ~V V = 0, we may make another choice for
C which leads us to a matrix C which does satisfy the skew-symmetry property.
Proposition 25 The following choice of C renders the expression _M� 2C to be
skew-symmetric.
C = HT�Th�M�~V � � ~V TM +M ~V
i�H (4.20)
Proof:
C = HT�Th�M�~V � � ~V TM +M ~V
iV
= HT�Th�M�~V � � ~V TM +M ~V
i�H _� (4.21)
= C _�
Substituting this new expression for C in _M� 2C gives us
_M� 2C = HT�Th�~V �T�TM +M�~V � + 2(~V TM �M ~V )
i�H (4.22)
which is now easily seen to be skew-symmetric.
Thus, eq. (4.20) is the correct choice for C if the skew-symmetry property is to be
exploited. The new expression for the matrix C also satis�es,
_M = C + CT : (4.23)
75
This result was �rst announced in [30]. Also at the same time in the doctoral thesis
[72], a new link level operator was constructed to derive a stacked operator expres-
sion for a matrix C which satis�es the skew-symmmetry property. The approach
presented here, however, is slightly more direct and does not require the de�nition
of any additional matrix operators.
IV.E Time Derivatives of Energy
It was last discussed with equation (4.6) how the kinetic energy of a me-
chanical system, such as the multibody system we have been studying, may be
expressed as KE = 12_�TM _� = 1
2V TMV . Similarly, the potential energy was seen
in (4.6) to be equal to PE =PNL
imi gc hC;i. In this section, we derive a recursive
algorithm for calculating the time derivative of the total energy,
TE = KE + PE : (4.24)
This quantity can often be useful for studying the evolution of a mechanical system.
In particular, at a single point one will be able to tell how the energy in the system
is increasing or decreasing.
Using the result found in equation (4.17), we have that
_(TE ) = _V TMV +1
2V T _MV + _V T
gMV : (4.25)
Note that when using the Spatial Representation V T _MV = 0 so that (4.25) has
the particularly appealing form of
_(TE ) = ( _V + _Vg)TMV (4.26)
Energy Time Derivative Algorithm
1. Compute �� and _Vg from forward dynamics algorithm.
2. Compute _V from inverse dynamics.
3. Combine them using (4.25) or (4.26).
76
IV.F Spatially Recursive Linearized Dynamics
The possible applications in control and estimation for the use of the
linearized dynamics are too numerous to mention here. Let it su�ce that the
existence of e�cient, spatially recursive algorithms for their calculation can be
of tremendous value. Though it is also possible to generate these equations using
software which can di�erentiate symbolically, this approach is often not feasible due
to the large memory and storage requirements as the dimension of the multibody
system increases. Additional impediments are the huge calculational processing
time necessary for such an undertaking and a closed form expression will often be
exceedingly large to work with. This \curse of dimenionality" can be experienced
with systems of dimensions already of 4 or 5.
In this section, we develop using the modeling framework presented so far
a spatially recursive form for the partial derivatives of the dynamics with respect
to the position and velocity states. The equations we present here were �rst given
in [42]. The equations can be applied to models which have joints which can be
represented as a series of single dof joints. We also add new explicit terms for
the gravitational forces. Later in Chapter V, the use of these equations for the
calculation of the adjoint equations in an optimal control setting are discussed.
Due to the techniques used here, these equations will only be valid using
the Spatial Representation. We exploit the unique property of this Representation
that at the point of calculation of the dynamics, the spatial force f acting on a
single rigid body is just the time derivative of the spatial momentum MV . This
property is used together with the identities found in Section A.D to obtain partial
derivatives of the system dynamics with respect to the state variables.
We present a di�erent spatial co-cross product operator similar to the
spatial cross product tilde operator for spatial vectors de�ned in Section II.A.1.
Let X and Y be spatial vectors. It was discussed in Section II.A.1 how spatial
cross product operators and vectors satisfy ~XY = �~Y X. The spatial co-cross
77
product operator will be denoted with a circle cross:
X =
264 c
d
375 ; c; d 2 R3 (X) =
264 ~c ~d
~d 0
375 : (4.27)
For brevity, this operator will be called the circle cross operator. It satis�es
the following identities which may be veri�ed through matrix multiplication and
the tilde identities in Section A.B.
(X)T = �(X) (4.28)
(X)Y = ~Y TX : (4.29)
As with the previously de�ned notational conveniences involving (6 � 6) spatial
matrices, when a circle-cross spatial quantity does not have any link reference index,
then this will indicate a block diagonal matrix with the 6 � 6 circle-cross matrix
operators of each link along the diagonal.
The following additional identities involving circle cross operators hold.
Let X be a constant spatial vector. Then,
Identity 26@
@�[HTX] = HT (X) ��H (4.30)
Proof: Using identity (A.31) and the identities of the circle cross operator (4.28)
and (4.29),
@
@�[HTX] =
@
@ _�[ _HTX] =
@
@ _�[HT (X)V ] = HT (X)�H = HT (X) ��H
The last equation comes from identity (A.9) and the fact that (X) is skew-
symmetric as indicated from (4.28).
Now, similarly consider X to be a constant spatial vector. Another iden-
tity is as follows:
78
Identity 27@
@�(MX) =
�� (MX) +M ~X
��H (4.31)
Proof: Again using identity (A.31) and the circle cross operator identities,
@
@�[MX] =
@
@ _�[ _MX]
=@
@ _�[(�~V TM �M ~V )X]
=@
@ _�[�(MX) +M ~X ]V
= (�(MX) +M ~X)�H
These identities will be useful in �nding the partial derivatives of the
inverse dynamics of the Spatial Representation: (2.61).
V = �H _�
_V = �(H �� � ~V �V ) = �(H �� + ~V H _�) = �d(H_�)
dt
f = �T�M( _V + _Vg)� ~V TMV
�= �T
d(MV )
dt+ �TM _Vg
u = HTf
(4.32)
The ability to write the inverse dynamics as a constant matrix times the derivative
of a spatial quantity allows for the possibility of deriving expressions for the partial
derivatives of the dynamics as we will soon see. The partial derivatives will �rst
be derived for the inverse dynamics, then later extended to the forward dynamics.
The �rst step will be to derive an intermediate result @
@�(MV ) which will be useful
in later calculations.
Identity 28@
@�(MV ) = �(MV )�H +M�( ~V � ~V �)H (4.33)
Proof: Using identities (A.31), (4.31), (2.8), (A.9), (A.26), and (4.13),
@
@�(MV ) =
@
@�(MX)jX=V +M(
@
@�V )
79
= [�(MX) +M ~X ]jX=V �H +M(@
@ _�_V �
d
dt
@
@ _�V )
= (�(MV ) +M ~V )�H +M@
@ _�(�H �� + �~V H _�)�M
d
dt(�H)
= (�(MV ) +M ~V )�H +M�( ~V H � ~V ��H)�M�~V H
= (�(MV ) +M ~V )�H +M(�~V � ~V �)H �M�~V �H
= �(MV )�H +M�~V H
The use of Identity (4.13) in the last step presumes a multibody system with only
single dof joints or one that can be modeled as such.
In the process of determining the partial derivatives of u, the term @
@�G
will be needed as u = HT�T d
dt(MV ) + G. We will be able to derive @
@�G with the
help of the subsequent intermediate results.
Identity 29 Let R and S be two block diagonal matrices containing spatial (6�6)
matrices on its diagonal which satisfy the recursive relationship Sk = Sk+1+Rk, or
equivalently, S = ET�SE� +R. Then S and R also satisfy
�TR� = �TS + S �� : (4.34)
Proof: Pre- and post-multiplying S = ET�SE�+R by �T and � and using Identity
(A.9) gives
�TR� = �TS�� �TS ��+ S �� = �TS + S �� :
De�ne fg as
fg = ��TM _Vg (4.35)
so that G = �HTfg. With the help of Identity (4.34), we arrive at the following
relstionships:
Proposition 30
�T (_z }| {
MV )� = �T (f � fg) + (f � fg)
�� (4.36)
�T (M _Vg)� = ��T (fg)
+ (fg) �� : (4.37)
80
Proof: Taking the circle cross operation of the recursions
f � fg = �Td
dt(MV ) = ET
�(f � fg) +
d
dt(MV )
fg = ��TM _Vg = ET�fg �M _Vg
results in
(f � fg) = ET
�(f � fg)
E� + ( _MV )
(fg) = ET
�(fg)
E� � (M _Vg) :
Associating the now block diagonal operators (f � fg) and (
_z }| {MV ) with S and R
respectively and using Identity (4.34) gives the �rst result. The second follows in
the same manner.
We are now ready to state one of the main results of this section.
Proposition 31 The partial derivative with respect to � of the vector of gravita-
tional forces G is given as
@
@�G = �HT�T
�(fg)
H +Mf_Vg�H� (4.38)
Proof: Using Identities (4.30) and (4.31) with equation (4.37), we have
@
@�G =
@
@�(HTX)jX=fg �HT�T
@
@�(MX)j
X= _Vg
= HT (fg) ��H �HT�T (�(M _Vg)
+Mf_Vg)�H
= �HT�T�(fg)
H +Mf_Vg�H�
The central results of this section will be the partial derivatives of the
applied forces u. As will be shown, the partial derivatives of the forward dynamics
will almost be automatic once those of the inverse dynamics are obtained. Jain
�rst presented this derivation in [42], though for single degree of freedom joints
and without any special treatment for the gravitational forces. Once again, our
ability to derive these results hinges upon the special characteristics of the Spatial
Representation which allows us to exploit the identities found in Section A.D.
81
Proposition 32 The partial derivatives of the inverse dynamics with respect to the
state (�; _�) are
@
@�u = HT�T
�� (f)H �M
~_Vg�H + 2 �M� _H +M� �H�
(4.39)
@
@ _�u = 2HT�T ( �M�H +M� _H) (4.40)
where �M = 12( _M � (MV ), _H = ~V H, and �H = � ~H _V + ~V ~V H. The operator (f)
is the block diagonal matrix with the 6� 6 circle cross operator of the spatial forces
on the diagonal.
Proof: We �rst prove the second statement, namely that of @
@ _�u. For this purpose,
we use Identities (A.31) and (4.33).
@
@ _�u = HT�T
@
@ _�[d
dt(MV )] +
@
@ _�G
= HT�T� ddt[@
@ _�(MV )] +
@
@�(MV )
�= HT�T ( _M�H +M� _H) +HT�T
�� (MV )�H +M�~V H
�= HT�T ( _M � (MV ))�H + 2HT�TM� _H
= 2HT�T ( �M�H +M� _H)
The derivation of @
@�u will require Identities (A.32), (4.30), (4.33), and (4.36), and
equation (4.38).
@
@�u =
@
@�[HT�T
d
dt(MV )] +
@
@�G
=@
@�(HTX)jX=(f�fg) +HT�T
d
dt
@
@�(MV ) +
@
@�G
= HT (f � fg) ��H +HT�T
d
dt[�(MV )�H +M�~V H] +
@
@�G
= HT (f � fg) ��H +HT�T [�( _MV )�H � (MV )� _H + _M�~V H
+M�~_V H +M�~V _H] +
@
@�G
= HT�T [�(f � fg)H + ( _M � (MV ))� _H +M� �H)
�HT�T ((fg) +M
f_Vg�)H= HT�T
�� (f)H + 2 �M� _H +M� �H �M
f_Vg�H�
82
Now, the forward dynamics are given by �� =M�1(u� C � G). As shown
in [42], we can obtain the partials of �� from those of the inverse dynamics.
@��
@�= �M�1@M
@�M�1(u� C � G)�M�1
@C
@�+@G
@�
!= �M�1@u
@�
@��
@ _�= �M�1@C
@ _�= �M�1@u
@ _�
We are therefore easily able to construct the partials of the forward dy-
namics.
Proposition 33
@
@��� = �[I �K H]D�1HT T
�� (f)H +M
f_Vg�H+2 �M� _H +M� �H
�(4.41)
@
@ _��� = �2[I �K H]D�1HT T ( �M�H +M� _H) (4.42)
Proof: Using equation (2.75) and Identity (A.13),
@
@��� = �M�1 @
@�u
= �[I �K H]D�1[I �K H]THT�T�� (f)H +M
f_Vg�H+2 �M� _H +M� �H
�= �[I �K H]D�1HT T
�� (f)H +M
f_Vg�H+2 �M� _H +M� �H
�
@
@ _��� = �M�1 @
@ _�u
= �2[I �K H]D�1HT T ( �M�H +M� _H)
83
The equations presented in this section are for the special case of single
dof joints, though certain types of multiple dof joints can also be modeled with
successive single dof joints. An example of an application of these methods is
presented in Section V.F when we outline how to recursively construct the adjoint
equations for the optimal control of a multibody system.
Chapter V
Nonlinear H2 and H1Control and
Numerical Solution Techniques
V.A Introduction
This chapter discusses various numerical techniques for the solution of
nonlinear H2 and H1 control problems with full state feedback. We present �rst
the de�nitions of these control formulations and give some indication of their impor-
tance in nonlinear control. Their linear analogues already have had much success in
the control community. Linear H2 or LQR control design dates back to Kalman's
work in the early 60's and continues to be frequently used. Linear H1 control
began to gain favor in the early 80's with the work of Zames [101], Helton [32],
and Doyle [20]. State space solutions for the problems were later presented in [19].
The continued popularity of H2 and H1 control methodologies stems largely from
their very good stabilization and robustness properties [102]. There are many types
of control problems which �t naturally into the H2 or H1 framework such as sta-
bilization problems, trajectory tracking, risk-sensitive problems, and disturbance
attenuation problems [28, 102].
The extension of the linear methods to nonlinear control problems fol-
lowed soon afterwards in the late 80's and early 90's with the pioneering work in
84
85
[7], followed by the work reported in [8, 37, 91]. In the nonlinear regimes, computa-
tional methods for solving these problems are still in their early stages, and there is
no generally accepted format for the best approach to solving them. The existence,
however, of numerical procedures which can solve these problems is vastly under-
appreciated. In Section V.D, we outline many of the proven numerical methods for
solving these problems. We pursue in Section V.C two such approaches for solving
nonlinear H2 and H1 control problems which we feel are well-suited to handling
saturation and other constraints of the problem, are e�cient, and can accomodate
higher dimensional systems to some extent.
After the discussion of the various existing numerical approaches, we il-
lustrate how our proposed numerical scheme is implemented by using the test case
of a swinging pendulum. Important subtleties of numerical optimal control become
very evident even in this case. For example, there may exist initial conditions of
the system for which there exist two completely di�erent, equally optimal solutions.
This can lead to situations where a small change in the initial conditions can lead
to radically di�erent solution trajectories.
The last section of the chapter, Section V.F, is more in the spirit of the
previous chapters on exploiting the recursive algebraic structure of multibody sys-
tems. It outlines how one may solve optimal control problems associated with
multibody systems via the method of characteristics. Using previous results from
Chapter IV on recursive, symbolic equations for the linearized dynamics, a recur-
sive and e�cient scheme is presented for the calculation of the adjoint equations
for this class of systems.
86
V.B Nonlinear H2 and H1Problems
The class of state equations that will be considered will be nonlinear,
smooth, time-invariant having the form,
_x = f(x; u;w) = a(x) + b1(x)u+ b2(x)w
z =
264 h(x)
u = l(x)
375 (5.1)
where x(t) 2 X is the state, u(t) 2 U is the controlled input, w(t) 2 L2[0,T], T � 0,
is the disturbance input, and z are the to-be-controlled outputs or penalties.
z
y = x
l(x)
u.
w
x = f(x,u,w)
Figure 5.1: System Diagram
V.B.1 Nonlinear H2Control and the HJB PDE
In the nonlinear H2 control problem, no disturbances enter into the model
(5.1), w � 0. The problem lies in minimizing a cost functional of the form,
V (x) = infu
�1
2
ZT
0jjz(t)jj2 dt+ '(x(T ))
�; (5.2)
which is often called the value function.
For numerical purposes, it is possible to incorporate the state equations
of the system into the cost functional, whereby traditional nonlinear optimization
software may be applied to it. One may also solve the problem by de�ning a
87
Hamiltonian function H(x; p),
H(x; p) = pTa(x)�1
2pT b1(x)b
T
1 (x)p+1
2hT (x)h(x) ; (5.3)
which in the H2 case is convex. The solution to the H2 control problem will satisfy
a Hamilton-Jacobi-Bellman inequality (HJB) de�ned by H(x; dVT
dx) � 0 [92]. In
the numerical methods to be discussed, we will be seeking the null Hamiltonian
solution where H(x; dVT
dx) = 0.
The solution of the control problem is thus \simpli�ed" to the solution of
a �rst-order PDE.
V.B.2 Bicharacteristic ODEs
It has long been known that this class of PDE's is equivalent to a system of
ODE's of double the dimension [9]. In the case of (5.3), the corresponding boundary
value problem is described by the Hamiltonian vector �elds dx
dtand dp
dt. The vector
p is denoted as the set of adjoint variables for the system, and it is also equal to
the transpose of the gradient of the value function, p = dVT
dx. The boundary value
problem consists of the following \bicharacteristic" equations:
_x = @H
@p(x; p) _p = �@H
@x(x; p)
x(0) = x0 p(T ) = @'(x(T ))@x
(5.4)
The Hamiltonian will be constant and equal to zero along an optimal trajectory,
thus solving the HJB inequality.
This boundary value problem characterizes solving the �nite time H2 or
H1 control problem with no �nal state constraints. We take the approach in this
paper of indirectly requiring the state x(t) to converge to a desired equilibrium x�
by including a penalty function of the squared error of the state from the equi-
librium into the performance. This has the e�ect that the system will converge
asymptotically to x� with speed determined by the gains chosen.
88
V.B.3 Optimal State Feedback Law
The vectors x and p may be used to de�ne the optimal control
u� = �bT1 (x) p (5.5)
along each optimal trajectory. The numerical values for x, p, and u make up an
open-loop solution as all variables will only be known as a function of time and
only at a discrete set of points along the trajectory. Once one has calculated many
such trajectories, one can begin to approximate the numerical function x 7! u,
known only on a grid, by a function u�(x) given for all x (at least all x in some
neighborhood) by a formula. This is done to interpolate between grid points, so
one has a control law which can be quickly applied to any state. An example of an
approximation method which can scale up to higher dimensions is a neural network
such as can be found in the easy-to-use Matlab neural network toolbox [62]. Later
in Chapter VI we use this to derive the optimal feedback controller for the jet engine
control problem.
V.B.4 Nonlinear H1Control
In the nonlinear H1 control problem, the model used (5.1) contains L2-
bounded disturbance inputs w. The objective here is to �nd a controller which
guarantees that the closed-loop system has an L2-gain less than or equal to a
prespeci�ed constant level from the disturbance inputs w to the output penalty
functions z [92].
We write this L2-gain condition as
ZT
0jjz(t)jj2dt � 2
ZT
0jjw(t)jj2dt+ �(x(0)) (5.6)
for all disturbance functions w(�) 2 L2[0; T ], T � 0. The constant value �(x(0))
satis�es 0 � �(x) � 1, and �(x�) = 0, where x� is the equilibrium for the system.
z(t) is the response of the system for the initial condition x(0).
89
As in eq. (5.2), one may de�ne the nonlinearH1 control problem as an op-
timization of a performance index. Unlike the H2 problem, however, the optimiza-
tion is of a minimax-type which prevents the use of standard nonlinear optimization
software.
One may similarly de�ne a Hamiltonian function H(x; p), which is now in
general nonconvex, for the nonlinear H1 control problem,
H(x; p) = pTa(x) +1
2pT�1
2b2(x)b
T
2 (x)� b1(x)bT
1 (x)
�p
+1
2hT (x)h(x) : (5.7)
The solution to this problem, as in the nonlinear H2 control problem, may be ob-
tained by solving for a C1 solution V (x) to the so-called Hamilton-Jacobi-Isaacs
inequality HJI , H(x; dVT
dx) � 0. The solution may also be calculated by solving a
set of bicharacteristic equations as in (5.4).
V.C Proposed Numerical Solution of Nonlinear H2 and H1
Problems
The numerical approach to be discussed consists of several methods which
may be used independently or in combination for solving the HJB and, in some
cases, the HJI equation. These methods will calculate the open-loop solution to an
optimal trajectory. The �rst method to be discussed is limited in that it can only
solve the nonlinear H2 optimal control problem.
V.C.1 Direct Minimization
To solve the H2 control problem numerically with the direct minimization
approach, one discretizes the state and control variables in time over the trajec-
tory. One may then formulate the minimization of the cost functional subject
to the di�erential and output constraints as a �nite-dimensional, nonlinearly con-
strained, optimization problem. The convexity of the H2 problem allows conven-
90
tional Newton-based methods such as Sequential Quadratic Programming (SQP) to
be used to solve the problem. Also, the direct minimization approach will �nd the
solution to the HJB equality,H(x; dVT
dx(x)) = 0. The program DIRCOL from Oskar
von Stryk [94, 95], which uses the nonlinear optimization software NPSOL (Gill,
Murray, Saunders, Wright [26]), takes this approach and, in addition, it provides
accurate estimates of the adjoint variables p.
V.C.2 Multiple Shooting
The second method, multiple shooting, may be applied to either the
H2 control problem or the H1 problem. The equations to be solved are the two-
point or multi-point boundary value problem described by (5.4). An important
property of this approach is that it can also handle nonconvex problems.
The prescribed boundary conditions for the problem are initial values of
the state x(0) and �nal values of the adjoint variables p(T ). The traditional shooting
method integrates the state equations beginning at x(0) and from a guess for p(0)
for a prespeci�ed time T . When the numerical value obtained for p(T ) from the
integration does not match the boundary condition p(T ) = @'(x(T ))
@x, a zero-�nding
problem may be set up in terms of the constraints, and Newton's method may then
be used to iterate on p(0) until the boundary condition is satis�ed.
The strong sensitivity of initial value problems, and the small convergence
region for the Newton's method make solving these problems via simple shooting
very di�cult. A multiple-shooting algorithm, however, is more stable and robust to
nonlinearities. This procedure consists of dividing up the time interval [0; T ] into a
series of grid points [0; t1; t2; : : : ; T ], and a shooting method is performed between
successive grid points (see Figure 5.2). A set of starting values for the state and
adjoint vectors is required at each grid point in time, and continuity conditions for
the solution trajectory introduce additional interior boundary conditions which are
then incorporated into one large zero-�nding problem to be solved.
The program MUMUS by Peter Hiltmann [33] is a multiple-shooting code
91
final curve
straight-linestarting values
x
time0 t t T
1 2
Figure 5.2: Multiple Shooting Method
which can solve two-point or multi-point boundary value problems using a modifed
Newton method. This method is, in general, quicker than direct minimization, lend-
ing itself more readily to iterated calculations of trajectories. A drawback, however,
is that the adjoint equations must be programmed as input to the code. For compli-
cated problems, generating the equations by hand or with symbolic manipulation
packages may pose a formidable task.
V.C.3 Shooting Out
While the multiple shooting method is commonly used for solving a bound-
ary value problem, one may alternatively obtain optimal trajectories by starting
near the desired equilibrium x� and at selected values p, then integrating the state
and adjoint di�erential equations outwards. This process is called shooting out. We
are, thus, able to obtain optimal trajectories for the state without having to solve
a di�cult two-point boundary value problem. A set of starting values (a guess for
the solution trajectory) is also not needed as is the case in direct minimization and
multiple shooting. This technique was successfully used by McEneaney in [63].
The disadvantage of the shooting out method is that one has less control
in reaching a speci�ed set of states. Remaining regions of the state space, that
92
are not able to be mapped out with optimal trajectories via shooting out, may be
mapped with the previous multiple shooting approach using as starting values the
optimal trajectories already obtained.
V.C.4 Combined Method
When one wishes to map out the value function over a large region of the
state space for purposes of approximating well the optimal closed-loop controller,
it is necessary to calculate a fair number of optimal trajectories. We can use the
superiority of direct minimization or shooting out methods to generate several of
these trajectories when little or no prior knowledge of the solution is available in
order to acquire starting values for the multiple shooting method when solving for
the remainder of the trajectories. The multiple shooting method works well when
many di�erent optimal trajectories are sought after since the algorithm is fast, and
one may initialize each run from the previous run. One has much more control in
the mapping out of the state space than one might have with shooting out, and the
method is, in general, quicker than direct optimization methods.
V.C.5 Homotopy
In order to compute the value function V and the optimal control u�, we
must compute a family of optimal trajectories which approximately (maybe only
coarsely) sweep out most of the state space. Supposing we have computed an
optimal trajectory Tx1 from a state x1 to the equilibrium x�, to quickly compute
trajectories Tx from many initial states x, simply pick x2 near x1 and use Tx1 to
initialize a MUMUS or DIRCOL computation of Tx2. We have found this proceeds
quickly to give us many optimal trajectories (see Section VI.B.5). Henceforth, we
refer to this as homotopying trajectories, since the point is to move them gradually.
When using the multiple shooting method, one may similarly homotopy
in the initial conditions of the state variables, using both the state and adjoint
trajectories previously calculated to serve as starting values for the new boundary
93
value problem to be solved. By calculating trajectories over the portion of state
space of interest, one will know u� and V at points over this entire area. Multivariate
approximation methods may then be used to �nd u� approximating u� as a function
of the state x.
After the calculations have been made, the approximation u� of the closed-
loop controller may then be implemented for use in real-time.
V.D Other Numerical Methods
There are many numerical approaches which have been tried on HJB and
HJI equalities and inequalities and listing them all is beyond us. We shall try to
list some of the main ones.
V.D.1 Finite di�erences
The method of �nite di�erences has been used successfully for solving HJB
and HJI problems of low dimension. For an account of �nite di�erence methods
and their relation to Markov chains see Kushner and Dupuis [54]. Also Falcone
[22] and the doctoral thesis by P. Dower under M. James [18] have used extensively
�nite di�erences in this setting. It is not clear how these would extend to high
dimensions in a manageable way, since rectangular grids have a rigid geometry.
V.D.2 "Small" basis of functions
The idea in this approach is to select basis functions fj , expand the value
function V asPM
j=1 cjfj , then some work is required to �nd the proper coe�cients cj.
The main di�culty lies in selecting the basis functions fj. The common approach
in conventional numerical PDEs (in dimension < 4) is to use basis functions which
are piecewise polynomials. In principle, in order to solve a �rst order PDE like
HJB's, they should be continuous. The tessellating (such as tetrahedra) of space
into pieces which match up is a serious problem. Doing this exibly in 3 dimensions
94
took substantial study, but of course is well developed now. The advantage is that
once everything is set up, mesh re�nement can be done on regions of space which
may otherwise seem treacherous. However, doing this in say 5 dimensions may well
be very di�cult.
One can select basis functions in a way which is \ad-hoc" and much less
di�cult. For example, Beard and McLain [10] use physical intuition coming from a
particular plant model to select 10 or 12 basis functions. For example, if the model
contains sines and cosines and powers of x, then their fj 's will too.
For control purposes, one need only solve HJI inequalities, provided the
solutions V satisfy conditions which make them what are called viscosity subso-
lutions. If continuity is not required for V and jumps are allowed, the problem
becomes easier, but the control law typically has jumps.
Once a basis of fj is selected, one can �nd the cj's in several ways. One
is a Galerkin procedure for solving HJI equations, described for example in [10].
Another is by directly substituting V =PM
j=1 cjfj into the HJI inequality to obtain
inequalities of the form
q(ffj : j = 1; : : : ;Mg; x) � 0 8xMXj=1
cjfj � 0 :
Here, q is quadratic in the fj , and one elects some �nite set of x to yield �nitelymany
constraints (crucial areas can be heavily gridded). Various optimization schemes
can be applied.
V.D.3 Partly Analytical Methods
Very intriguing is backstepping, which applies to special classes of systems
and obtains analytical expressions for V and the controller u. Thus, when it applies,
it has the potential for beating the curse of dimensionality. One advantage of
obtaining analytical expressions for V and u is that often the solution need not be
recomputed with changes in the parameter values or choice of equilibrium.
95
Backstepping does not approach saturation directly, however, although a
highly experienced practitioner may well come up with compromises. This makes
it di�cult to compare our e�orts with backstepping, especially in the jet engine
compressor problem considered in Chapter VI where we will see that rate saturation
dominates.
Similarly, feedback linearization is not able to address saturation directly.
V.D.4 Characteristics and Direct Minimization
The method of characteristics, as they pertain to multiple shooting and
shooting out, and direct minimization methods were presented in the previous
section. They seem to have the most promise among methods for \fully" solving
HJB equations. The shooting out or multiple shooting methods can be a bit di�cult
to enter into a program at this point, since the adjoint equations can be complicated.
Direct methods, while slower, are comparatively easy to run. All of these methods
�nd the value function V (x) at each point x along a curve. In delicate regions, one
can pick many x's, otherwise only a few x's may be necessary to approximate V (x)
well. A careful approach can, thus, help combat the curse of dimensionality.
The �nal desired result is a \simple" formula to approximate the function
V in order to obtain a control law. Traditional Galerkin methods reverse this in
that they layout the approximation scheme �rst (before having any information
about V ). Since the Galerkin scheme must mesh very well with the numerical PDE
one is solving, this is much harder to do than merely approximating V given as
data on a grid.
V.D.5 Miscellaneous
Other approaches include the technique using viability kernels as promoted
by P. Saint-Pierre [82]. Bertsekas and Tsitsiklis also solve HJB and HJI equations
for discrete problems in their book [12].
96
V.E Illustration: Pendulum
In order to demonstrate the basics and subtleties of applying these nu-
merical techniques to a control problem, we present here a brief tutorial example of
the optimal control of a pendulum. Already in this simple problem, one �nds many
numerical particularities which are important. For example, we discover multiple
sheets of the pendulum. These are regions of the state space where numerically
calculated optimal trajectories will stay within when iterating on the initial condi-
tions. Yet as one progresses far enough along the sheets, it may be the case that
the calculated trajectories are no longer optimal.
Later, when we deal with the more complex problem of jet engine com-
pressor control, we must, for example, be careful to rule out multiple sheets or to
�nd them. As we shall see in that case, extensive searching did not reveal any such
anomalies.
The dynamical equations of motion of a pendulum are,
ml2 �� +mg l sin(�) = u
where � is the position angle of the pendulum with respect to the bottom `rest'
position. The pendulum rod is assumed to be massless of length l with a point
mass of m at its end, and g is the gravity constant. See Figure 5.3.
theta
Figure 5.3: Pendulum
97
Let x1 = � and x2 = _�, and for simplicity let l = m = 1. Then we write
the state and output equations as
_x1 = x2
_x2 = �g sin(x1) + u; z =
264 (x� xf )
(u� uf )
375 ; (5.8)
where xf is a desired equilibrium of the pendulum and uf is the control e�ort at
the equilibrium. We specify a weighted quadratic cost functional to be optimized
as
minu
1
2
ZT
0
nkx� xfk
2Q+ ku� ufk
2R
odt+ '(x(T )) (5.9)
where Q;S � 0 and R > 0 are square matrices of appropriate dimension, and the
norm kzk2Qsatis�es zTQz for some vector z and matrix Q. The penalty function
on the �nal state is equal to '(x(T )) = 12kx(T )� xfk2S .
We wish to solve the HJB equality (5.3), H(x; dVT
dx) = 0, for V . From the
Hamiltonian, we construct the bicharacteristic equations (5.4) with initial and �nal
boundary conditions
x(0) = x0; p(T ) =@'
@x(x(T )) = S(x(T )� xf) :
Once these are solved, a closed-form expression for the optimal control u�(x) along
the bicharacteristic trajectory can be determined from p = dVT
dx(x) and the extremal
value for u�,
u� = �R�1 b1T (x) p : (5.10)
V.E.1 Nonlinear H2 Solution and Comparison of Numerical Methods
We compare both the direct minimization approach (DIRCOL) and the
multiple shooting method (MUMUS) in our numerical experiments. Of interest are
their respective run-times, the e�ects of varying numbers of grid points (in time),
and their convergence properties.
With the multiple shooting code MUMUS, for example, it was found that
run-times are not as a�ected by the choice of discretization as the optimization
98
approach of DIRCOL. Also, given a good set of starting values, MUMUS can reach
the solution very quickly. The di�culty with MUMUS lies in obtaining reasonable
starting values, in particular, those for the adjoint variables. For the pendulum,
perhaps due to its low dimension and relatively mild nonlinearities, MUMUS was
able to converge well even with the obvious choice of straight-line starting values.
When iteratively calculating many trajectories with large numbers of grid points,
MUMUS thus was the obvious choice.
The direct optimization package DIRCOL makes it much easier for the
user to de�ne the problem and to enter in nonlinear equality and inequality con-
straints when desired. The use of this approach is very useful in a preliminary
analysis of an optimal control problem, even one as simple as the pendulum. Also,
the HJB equality in the nonlinear H2 control problem may have more than one
solution. An optimization code will converge to the stabilizing solution of the HJB
equality, while it is not guaranteed that the shooting code will do the same. Since
for small numbers of grid points the run-times of the two methods are very similar,
a natural approach is to solve the problem initially with DIRCOL or by shooting
out, then to initialize MUMUS with its output. This was the combined method
previously described in Section V.C.4.
V.E.2 Multiple Sheets of the Value Function
The pendulum has been a heavily studied test problem. Even in our case,
this example sheds light on some rather interesting properties of optimal control
and of our choice of numerical methods. When applying the previously described
homotopy method, we discover sheets along which the multiple shooting method
will continue to produce solutions to the H2 control problem, though the calculated
trajectories may no longer be optimal.
In our analysis, the state x1 represents the position angle which determines
the con�guration of the system and x2 is the angle velocity. In the coordinates
chosen, x1 = 0:0 refers to the bottom `rest' position of the pendulum, and the
99
−4 −3 −2 −1 0 1 2 3−15
−10
−5
0
5
10
15
x1
x2
Figure 5.4: Plot of optimal trajectories calculated in the state space to the equilib-
rium x = (�; _�) = (�1:5; 0).
desired �nal equilibrium is chosen as xf = (xf1; xf2) = (�1:5; 0) 1.
In order to calculate V and u, the homotopy method is used with the
multiple shooting program MUMUS to sweep out a large region of the state space,
repeatedly calculating trajectories with slightly varying initial conditions. Figure
5.4 displays the calculated trajectories. the various trajectories. We take the opti-
mal trajectory with initial condition (x1(0); x2(0)) as an initialization for another
run of MUMUS with initial condition (x1(0)+ �1; x2(0) + �2). The choice of (�1; �2)
will determine how accurate the resulting value function and control law will be.
In our experiment, a homotopy was performed over a large region of state space,
roughly �3:9 < x1 < 2:4; �11 < x2 < 14 : The �nal time T , to which the trajec-
tories are calculated, was chosen so that all of the optimal trajectories converged to
within a small tolerance level of the equilibrium. Finding an appropriate T required
some trial and error.1All angles are in radians.
100
−6 −5 −4 −3 −2 −1 0 1 2 3 40
50
100
150
200
250
x1
V
Figure 5.5: Homotopy in x1(0), (x2(0) = 0) along two sheets of pendulum. The
dotted lines indicate the continuation of a sheet along which the numerical methods
will continue to calculate solutions though they are no longer optimal.
Our experiment consisted of two phases. The initial phase consisted of
a homotopy in x1(0) along the two primary sheets of the pendulum. One sheet
traverses the pendulum in the clockwise direction iterating the initial condition
(x1(0); x2(0) = 0) in the negative x1 direction. Each run uses the previous run as
its initialization. The same is done in the counter-clockwise direction. Since x1 is
periodic, eventually the homotopy will calculate trajectories on two di�erent sheets
corresponding to the same initial condition. These two trajectories will be di�erent
since they will correspond to traversing the pendulum in opposite directions to
reach the desired �nal endpoint and; hence, they will have di�erent values for the
value function. Recall from eq. (5.2) that the value function V (x) is the in�mum
of a cost functional over all possible trajectories. The `true' V will be the lowest
of all the values obtained, while the higher values are thrown out as they are not
`optimal.' More details on combining di�erent calculated values for V to construct
101
the true value function are described by McEneaney [63].
rest
final
boundaryequal-cost
start(-1.5,0)
(1.0,0)
(0,0)
(2.4,0)
Figure 5.6: Equal-cost Boundary on Pendulum
Figure 5.5 represents a vertical slice along the x1 axis of the value function.
On the vertical axis is the value function V which represents the cost in reaching
the desired equilibrium x�. This cost is de�ned by the quadratic performance (5.9).
Note that since x1 is periodic in 2� � 6:3, x = (�3:9; 0) is the same point as x =
(2:4; 0). The value function has the same value there, V (�3:9; 0) = V (2:4; 0) = 185.
At this point, it is equally costly for the pendulum to travel counter-clockwise as it
is to travel clockwise in order to reach the desired equilibrium as shown in Figure
5.6. The state space is not a simply connected region, and it is therefore to be
expected that there exist points from which two equally costly and optimal paths
exist to the desired �nal state.
In the second phase, we iterate the initial values of the trajectories we wish
to compute in such a manner as to keep (x1(0); x2(0)) near a level set of V (x). This
way of choosing (x1(0); x2(0))'s is not essential but easy to implement. In Figure
5.7, we have graphed the value function for V (x) � 185. Using an approximation
method, we then obtain the desired closed-loop control law.
102
−4−2
02
4
−20
−10
0
10
200
50
100
150
200
x1x2
V
Figure 5.7: Value function of pendulum.
V.F Recursive Calculation of Robot Adjoint Equations
We have discussed how the solution to a nonlinear H2 or H1 optimal
control problem satis�es a Hamilton-Jacobi-Isaacs equation as well as a 2-point
boundary value problem described by the state equations and a set of adjoint equa-
tions (5.4). The boundary value problem may be obtained with the optimal control
u�(x; p), and worst-case disturbance w�(x; p) in case of an H1 control problem,
from the Hamiltonian H,
_x =@H(x; p; u�; w�)
@p; _p = �
@H(x; p; u�; w�)
@x: (5.11)
These equations may be represented entirely in terms of the state x and the adjoint
variables p.
In recent years, powerful multiple shooting algorithms that can solve these
boundary value problems have been used with greater frequency [50, 29, 63], though
the task of generating the adjoint equations can often be a formidable obstacle. This
has also been the case with multibody systems where the size and complexity of
the equations of motion increases at a tremendous rate with increasing dimension.
103
The adjoint equations which involve the partial derivatives of the dynamics with
respect to the state variables will even be more lengthy and complex. Symbolic
manipulation packages have at least made available the possibility to obtain these
equations, though storage and computational considerations place severe limits on
the dimension of the problem as the entire set of equations in closed form are
needed.
We, thus, recognize the utility of recursive symbolic expressions for the
calculation of the adjoint equations of a general robotic system. Using the equations
presented earlier for the recursive, symbolic calculation of the linearized dynamics
in Section IV.F, we describe how the adjoint equations for a multibody system
may be calculated recursively using the symbolic matrix operators in an e�cient
manner. The ability to recursively calculate the adjoint equations for this class of
systems in a manner which does not su�er the common problems related to the
\curse of dimensionality" opens up many possibililties for using numerical optimal
control techniques on these systems. These tools and ideas have not previously
been presented in the research literature.
As previously presented in the context of nonlinear H2 or H1 optimal
control, one has a performance index to be optimized of the form
V (x) = minu
maxw
ZT
0L(x(t); u(t); w(t)) dt ; (5.12)
where L(x; u;w) is the integrand of the performance index for the system, and u
and w are the control and disturbance respectively. In the case of an H2 problem,
w � 0. The state x is de�ned in the case of our robotics system as x = [xT1 xT
2 ]T .
De�ne similarly a set of adjoint variables p = [pT1 pT
2 ]T of the same dimension so
that the Hamiltonian function is de�ned as
H(x; p) = pT
264 x2
M�1(u+ w � C � G)
375+ L : (5.13)
104
Using (5.11), the bicharacteristic equations for the multibody system are
_x1 = rp1H = x2
_x2 = rp2H = M�1(u+ w � C � G)
_p1 = �rx1H = �( @
@�
��)Tp2 �r�L
_p2 = �rx2H = �p1 � ( @
@ _���)Tp2 �r _�L
(5.14)
The partial derivatives of the integrand of the performance,r�L and r _�L,
are dependent on the form of L. In standard cases, L is quadratic in x, u, and w
(see (5.6)). Thus, its partial derivatives will be only linear in x as we can subsitute
for u�(x; p) and w�(x; p) after taking partial derivatives. The equations for _x are
merely the forward dynamics presented in Section II.G with the optimal values u�
and w� substituted for u and w. The form for u�(x; p) was given in (5.5) while the
form for w�(x; p) follows similarly by taking the partial derivative with respect to
w of the Hamiltonian. Assume that L is quadratic in its arguments so that the
Hamiltonian has the form
H(x; u;w) = pT (a(x) + b1(x)u+ b2(x)w) + xTx+ uTu� 2wTw : (5.15)
Then we can establish the following result:
Proposition 34 A recursive, symbolic matrix expression for calculating the ad-
joint equations of a multibody system is given from the general form of the adjoint
equations (5.14), the optimal values for the control u� and disturbance w�,
u�(x; p) = �bT1 (x)p (5.16)
= �M�1p2 = �[I �K H]D�1[I �K H]Tp2 (5.17)
w�(x; p) =1
2bT2 (x)p (5.18)
=1
2M�1p2 =
1
2[I �K H]D�1[I �K H]Tp2 (5.19)
and the recursive, symbolic expressions for @
@�
�� and @
@ _��� given in Section IV.F which
when combined with p2 gives
(@
@���)Tp2 =
��HT (f)T +HT�T
g_V T
gM + 2 _HT�T �M
105
+ �HT�TM� HD�1[I �K H]Tp2 (5.20)
(@
@ _���)Tp2 = (HT�T �M + _HT�TM) HD�1[I �K H]Tp2 (5.21)
These recursive equations may be implemented in a manner analogous to
the equations presented in Chapters II, III, and IV. The values for u� and w� can
be calculated in two sweeps of the multibody chain which when combined with the
forward dynamics will add an additional two sweeps to the three normally required
to calculate _x. The additional calculation for the adjoint equations _p requires four
sweeps of the multibody chain and can, thus, be performed in parallel with the
augmented forward dynamics algorithm.
V.G Notes
The text of Chapter V, in part, is a reprint of the material as it will appear
in IEEE Control Systems Technology under the title \Numerical Solution of Non-
linear H2 and H1Control Problems with Application to Jet Engine Compressors,"
and with authors, Michael Hardt, J. WilliamHelton, and Kenneth Kreutz-Delgado.
The dissertation author was the primary researcher and author and the co-authors
listed in this publication directed and supervised the research which forms the basis
for this chapter.
Chapter VI
Case Study: Optimal Control of
Jet Engine Compressors
VI.A Introduction
We apply our proposed numerical approach to the control of a jet engine
compressor. The 3-D Moore-Greitzer model is used as a basis for studying the
compressor system [65], and we use throttle feedback control to robustly stabilize
the system under the constraints and performance criteria imposed. We calculate
the solution of the nonlinear H2 control problem subject to rate saturation con-
straints and discuss considerations and di�erent approaches to the problem in light
of undesirable transient behavior which can arise. The transient behavior of special
concern is a large drop in the plenum pressure rise , and these are punished by
the H2 performance criterion.
Our studies indicate that the choice of performance index or cost function
to be optimized is largely secondary to the strong e�ects of actuator rate saturation.
The strong in uence of rate saturation is best displayed later with Figure 6.4 in
our discussion of the jet engine compressor. We describe how one may construct a
globally optimal controller which meets these rate constraints. The performance of
our �nal controller is then graphically displayed with contour plots of the resulting
106
107
value function and drops in the plenum pressure in di�erent operating regions. This
work represents the �rst time H2 optimal controllers were computed for this com-
pressor problem. The results in this paper were �rst announced in [29]. Previous
research also did not expressly consider the important e�ects that rate saturation
might have on the problem.
VI.B Jet Engine Stall and Surge Control
The three state Moore-Greitzer model of a jet engine compressor has been
frequently used in the literature to study the severe instabilities found in this class
of systems. Surge is the name given to oscillations in the annulus averaged ow
of the compression system while rotating stall is a disturbance which consists of
regions of reduced or reversed ow that rotate around the annulus of the compressor
[21]. The speci�c model that we will be using is [53]:
_� = �+c � 3�R
_ =1
�2(� + 1�
p) (6.1)
_R = �R(1� �2 �R) ; R(0) > 0
The meaning of the state variables is as follows:
� - the annulus - averaged mass ow coe�cient
- the plenum pressure rise
R - the squared amplitude of circumferential ow asymmetry
c - the compressor characteristic relating pressure rise in the plenum to the
mass ow �
- proportional to the throttle area (control)
�; � - system-dependent parameters
108
inlet outlet throttle
compressor
plenum
Figure 6.1: Jet Engine Compressor
A common approach in constructing a model for the system is to represent
c as a cubic in � which is only an approximation to the actual phenomenon. This
is a hypothetical approximation to the true compressor characteristic since the
region (� � 1) is unstable and cannot be physically observed, while in the stable
region (� > 1) experimental data is usually used in practical settings to arrive at
the compressor characteristic. A common form of the cubic approximation is:
c = co + 1 +3
2��
1
2�3 :
The peak operating point of the compressor coincides with the peak of
the compressor characteristic in the region f� � 0g at � = 1. At this point the
system's linearized equations are only marginally stable when using the throttle
parameter as the control input. However, it has been shown that the system may
be stabilized globally at this equilibrium using a nonlinear feedback [53].
There exists a continuum of stable equilibria in the region (� > 1). The
equilibria in this region can be parametrized by � = �0,2666664�
R
3777775f
=
2666664
�0
c(�0)
0
3777775 :
The linearized Moore-Greitzer model is uncontrollable at these equilibria, but it
is stabilizable. The model at the R = 0 or no-stall equilibria, prevalent in the
109
(� > 1) region, cannot be stabilized by throttle feedback in the region (� � 1).
There does, however, exist an unstable set of equilibria in this region, termed the
stall-equilibria, for which the model can be stabilized by throttle feedback,
2666664�
R
3777775f
=
2666664
�0
c � 3(1 � �02)�0
1� �02
3777775 :
Here, the linearized equations are controllable, though the open-loop system is
unstable.
VI.B.1 Problem Statement
We shall use numerical methods to solve the nonlinear H2 control problem
thus obtaining an optimal control law of the form �(�;; R). We began the anal-
ysis by studying a proper choice of performance index or cost function. There were
two main problems also encountered at the outset which are fundamental to the
system and were not able to be remedied by guessing di�erent performances. The
�rst main di�culty that we encounter is what we call theMinimum- Problem.
This involves a large dip in the pressure variable along the closed-loop trajectory
resulting from the controller . An objective is to design a controller which will
keep the system trajectory inside a given neighborhood N of the chosen equilib-
rium. Another major di�culty is the Rate Saturation Problem. The controller
must meet speci�ed bounds on the throttle rate _ . Also, an absolute bound on
is required, but our investigations lead us to suspect that this constraint often
becomes inactive with realistic bounds on _ . Both problems will worsen with larger
initial values of R.
Another practical problem is to �rst choose a high-performance equilib-
rium de�ned by �0 and a control law (�;; R) which has the property that all
states near to the equilibrium are kept close to it. We illustrate how one may com-
pute a set NI of states with the property that a closed trajectory starting at a state
110
in NI stays completely inside the speci�ed neighborhood N .
In our model, we chose as parameters � = 4, � = 1, �co = 0:1469. The
error coordinates of the system with respect to the no-stall equilibria (� > 1) will
be de�ned as � = (� � �0), = ( � c(�0)), r = R. Also f will denote the
equilibrium control input. We began the analysis by studying a proper choice of
performance index or cost function.
VI.B.2 Choice of Performance Index
A preliminary investigation did not directly impose rate saturation con-
straints and focused on the maximum pressure equilibrium (�0 = 1). The goal was
to test various quadratic and non-quadratic performance indices in order to �nd
an optimal controller having good transient properties remedying the Minimum-
and Rate Saturation Problems. This approach turned out to be unsuccessful.
The general form of the quadratic performance index used was
min
ZT
0
nc1�
2 + c2 2 + c3r
2 + c4( � f )2odt ; (6.2)
while that of one with cross-terms was represented by
min
ZT
0
nc1�
2 + c2( � c5�)2 + c3r
2 + c4( � f )2odt : (6.3)
Placing higher powers on the penalties of the state variables actually worsened
the Minimum- Problem. Thus, in the remainder of the investigation we used a
quadratic performance index with all weights set equal to 1. Additionally, rate
saturation constraints will always be imposed.
VI.B.3 Rate Saturation Constraints
Either numerical approach (DIRCOL/MUMUS) lends itself easily to im-
posing rate saturation constraints. A dummy state is introduced which is equal to
the control input. This then gives an additional state equation with its derivative
equal to the control rate which becomes the new control for the system. In the
111
0 2 4 6 8 10 12 14 16 18 201
2
3
4
5
6
7
Time
u1
Figure 6.2: Control vs. time for the H2 optimal trajectory and di�erent perfor-
mance measures. The solid line is with respect to a quadratic performance index,
and the dashdot and dashed lines are with respect to performance indices with
cross-terms with weights c5 = 1 and 5 respectively. The circles represent the sys-
tem initialized at R = 0 with a quadratic performance.
case of the multiple shooting algorithm, a box constraint on the control rate _
simply translates into an if statement when the control law is de�ned. To simplify
the problem de�nition, we added a quadratic term in the control rate _ to the
performance index. This permits us to use the standard approach of setting the
partial derivative with respect to the control of the Hamiltonian function to zero
and thereby arrive at an expression for the optimal level of _ . The performance
index
min_
ZT
0
nc1�
2 + c2 2 + c3r
2 + c4( � f )2 + c5 _
2odt (6.4)
is then used for the remainder of our analysis.
Figure 6.3 shows the results of imposing a constraint on the control rate
_ with the constraints j _ j � 0:1 and j _ j � 0:2. The control is restricted to
112
0 2 4 6 8 10 12 14 16 18 201.5
1.6
1.7
1.8
1.9
2
2.1
2.2
time
Psi
Figure 6.3: vs. time illustrating e�ects of introducing di�erent constraints on _ ,
the control rate. The solid line is the unconstrained trajectory while the dashdot
and dashed line represent a constraint of j _ j � 0:2 and j _ j � 0:1 respectively.
start at its �nal equilibrium value since in practice the controller cannot change
values discontinuously. Evidence of the constraint is seen in the slightly larger dip
that the pressure is forced to take. This may be considered to be the price for
rate saturation. The initial conditions for this experiment were � = 1:1, = 2:0,
R = 0:2, and the desired equilibrium is �0 = 1:2, c(�0) = 2:083, R = 0 where the
equilibrium pressure lies at 97% of its maximum value of c(1:0) = 2:1469. In the
remaining portion of the paper, we will continue to use a rate saturation constraint
of j _ j � 0:1 together with a quadratic performance index.
VI.B.4 Choice of Equilibrium
In an attempt to improve the Minimum- Problem, a tradeo� was ex-
plored by choosing di�erent equilibria in the no-stall region (�0 > 1) along the
113
compressor characteristic. The bene�t of converging to an equilibrium further
down the characteristic curve is that the dip in the pressure and the control
rate _ along an optimal trajectory will be greatly reduced.
0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.51.5
1.6
1.7
1.8
1.9
2
2.1
2.2
Phi
Psi
Figure 6.4: Controlling to di�erent equilibria along compressor characteristic at
a starting initial value of R = 0:12. Trajectories move in a counter-clockwise
direction.
Figure 6.4 pertains to the H2 optimally controlled system with the rate
saturation constraint j _ j � 1. We study the in uence of this rate saturation con-
straint with four di�erent equilibria along the compressor characteristic (solid line).
The �gure displays the behavior of the closed-loop system after initializing at points
near the chosen equilibria. The state of the closed-loop system follows a trajectory
(moving in a counter-clockwise direction) which eventually ends in that equilibrium;
these are the loops we see plotted. More speci�cally, the four equilibria that we con-
sider are (�k;k; Rk) = (�k;c(�k); 0), with �k equal to 1:1; 1:2; 1:3; 1:4 and with
respective initialization points at (�k;k; 0:12) for k = 1; : : : ; 4. Thus, the system
begins at R = 0:12 signifying a modest size stall level which must eventually reach
114
its equilibrium value of R = 0.
Striking is that the closed-loop optimal trajectory converging to the �1 =
1:1 equilibrium exhibits a very large drop in pressure before convergence. In con-
trast, the equilibriumwith �2 = 1:2 has a pressure which is at 97% of the maximum
possible pressure, and the trajectory initialized by our modest displacement from
it has only a small pressure drop. The explanation for this dramatic change in
performance of our optimal H2 controller is that the rate saturation constraint be-
comes active for a signi�cant time along the trajectory converging to the �1 = 1:1
equilibrium, and does not become active along the other trajectories.
Table 6.1: Comparison of di�erent equilibria.Perc. Min Max Max
�k c(�k) c(1:0) j _ j
1.1 2.131 0.993 1.520 1.652 0.1001.2 2.083 0.970 1.975 1.536 0.0721.3 1.998 0.931 1.946 1.627 0.0181.4 1.875 0.873 1.841 1.753 0.014
Table 6.1 compares key features of the trajectories in Figure 6.4. Listed are
the values of the desired mass ow �k which de�ne the equilibria, the equilibrium
pressure c(�k), the percentage of the maximal equilibriumpressure (1:0) that the
desired equilibrium lies at, the minimum value that reaches along the trajectory,
and the maximal value of and j _ j.
VI.B.5 Calculation of Value Function
For purposes of calculating a value function (5.2), we chose as a desired
equilibrium �0 = 1:2, where the pressure is at 97% of the maximum equilibrium
pressure. Furthermore, we assumed a rate saturation constraint of j _ j � 0:1.
Then 381 trajectories were calculated from various initial values (�;; R) to the
equilibrium (�0;c(�0); 0). The initial values were equally spaced with ranges
� 2 f1:0; 1:4g, 2 f1:6; 2:2g, and R 2 f0:0; 0:2g. The �nal time T to which
115
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.41.6
1.7
1.8
1.9
2
2.1
2.2
Phi
Psi
V = 0.018316
V = 0.049787
V = 0.135335
V = 0.367879
V = 1.000000
V = 2.718282
V = 7.389056
Figure 6.5: A contour plot of a slice of the value function at fR = 0:12g. The �
indicates the equilibrium.
the trajectories were calculated was set at 20.0 seconds, which allowed ample time
for the system to converge from any state in the given region. The average time
to calculate each trajectory with MUMUS was 6.3 sec. on an old SPARC IPX.
Varying the grid size in MUMUS was at times also necessary to ensure convergence
from initial values in the state space, since areas of extremely high curvature in the
trajectories cause numerical troubles. Since each trajectory contains the open-loop
value for the control law, the various trajectories were needed to calculate a rea-
sonable approximation for the closed-loop control law. A slice of the value function
at fR = 0:12g is displayed in Figure 6.5.
Trajectories calculated from some areas of the state space exhibit poorer
transient properties than from others. In particular, the closer the initialization is
to the peak of the compressor characteristic, the more di�culty is encountered in
controlling R to 0. In the presence of the rate saturation constraint j _ j � 0:1 and
starting close to the peak, the optimal trajectory would allow a substantial increase
116
in R before controlling it to 0. Such transient behavior may often be impractical
or nonimplementable as the growth in R signi�es the growth of the stall cell. This
e�ect becomes more pronounced with larger initial values of R. We see this as a
consequence of the rate saturation constraint which prevents the controller from
keeping the stall instability from growing.
VI.B.6 State Feedback Control
After the calculation of the value function, there are a number of methods
which may be used to approximate the control law in equation (5.5). The calculated
trajectories provide us with numerical values of �, ,R, and � on a grid in the state
space. One may then approximate the data �(�;; R) with the feedback control
law �(�;; R) as described in Section V.B.3. In [71], it was shown how a neural
network may e�ectively be used to approximate the closed-loop controller in this
manner. We took this same approach using a 2-layer, 10 node feedforward neural
network to approximate �(�;; R) with very good results. This approximation
was done o�ine using the Matlab neural network toolbox. The approximation was
done in a large box on the scale of that displayed in Figure 6.5 and with R in the
interval between 0:0 and 0:12. Only several points were excluded near the extreme
left uppermost corner of the box where the system was approaching a full stall.
The resulting weight and bias matrices for the �rst and second layer are
w(1) =
26666666666666666666664
�10:2954 6:2032 9:8482
�6:6018 1:4250 �9:6758
�2:9512 �2:5187 3:1642
12:3135 �6:8863 0:5173
10:8508 �7:6563 �2:5987
�6:0692 0:1712 12:8845
�8:8322 8:2157 10:8908
9:4260 �6:2197 �11:2955
�3:4612 1:4363 �4:0186
�0:8463 �1:2278 4:4439
37777777777777777777775
117
b(1) =
26666666666666666666664
�3:2812
12:4888
7:1970
0:7974
0:7149
�5:9826
�6:2920
4:5572
5:0425
5:7511
37777777777777777777775
w(2)T =
26666666666666666666664
1:5127
7:0670
0:4396
0:2139
0:0926
�0:0196
0:2830
1:3205
�3:0503
�3:4664
37777777777777777777775
and b(2) = �1:1896. Thus, the controller is given as follows.
Let x = [� R]T represent the state vector. Then the control law is
�(x) =10Xi=1
w(2)izi + b(2) ; (6.5)
where the values zi (corresponding to the 10 hidden nodes) are calculated
as
zi = g(3Xj=1
w(1)ijxj + b
(1)i) (6.6)
using the logistic sigmoid function g(a) = 11+e�a
. Note that the numer-
ical online burden for implementing this controller is at each time step
10 function evaluations and about 80 ops.
VI.B.7 Evaluation of Results
Figures 6.6,6.7,6.8 best demonstrate the e�ectiveness of the controller con-
structed in the previous section. Figure 6.6 gives a good indication about the quality
of Minimum- performance one may theoretically expect with the implementation
of this feedback control law. In it we initialize R(0) to 0:12 and choose as a de-
sired equilibrium (�0;c(�0); R) = (1:2; 2:083; 0:0) which lies at 97% of the peak
operating pressure. The white neighborhood in the �gure, for example, contains
the set of initial states with the property that the controller � drives these to the
equilibrium � along a trajectory upon which remains above 2:0. Likewise, the
next shaded neighborhood consist of states which are � controllable with pressure
always remaining above 1.95.
118
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.41.6
1.7
1.8
1.9
2
2.1
2.2
Phi
Psi
Min Psi >= 2.00
Min Psi >= 1.95
Min Psi >= 1.85
Min Psi >= 1.75
Min Psi >= 1.65
Min Psi >= 1.30
Min Psi >= 0.90
Min Psi >= 0.60
Figure 6.6: A contour plot of the minimum values of attained along closed-loop
system trajectories initialized at di�erent (�;) and at R = 0:12 in the state space.
Figure 6.7 displays max j���0jwhen starting from di�erent points through-
out the state space while Figure 6.8 contains the maximum values of R when start-
ing from R(0) = 0:12. Note that in the majority of the state space, R decreases
monotonically with implementation of the optimal closed-loop controller result-
ing in the large region labeled by R � 0:12. Only when initializing in the the
upper-left corner of the (�;) plane near the peak of the compressor characteris-
tic (�0 = 1:0;c(�0) = 2:147) does the nice convergence of R deteriorate. When
initializing at (�0 = 1:0;c(�0) = 2:147; R = 0:12), R increases to 0:88 which
symbolizes almost a full stall of the compressor. Fortunately, this was the only
point in the region shown where the closed-loop controller was not able to prevent
an excessive growth in the value of R. A similar e�ect is shown in Figure 6.7 where
the deviations from the equilibrium value of �0 become much larger near the peak
of the compressor characteristic.
The three �gures allow one to determine neighborhoods of initial values
119
1 1.05 1.1 1.15 1.2 1.25 1.3 1.35 1.41.6
1.7
1.8
1.9
2
2.1
2.2
Phi
Psi
abs(Phi − Phi_0) <= 0.13
abs(Phi − Phi_0) <= 0.25
abs(Phi − Phi_0) <= 0.38
abs(Phi − Phi_0) <= 0.53
abs(Phi − Phi_0) <= 0.69
abs(Phi − Phi_0) <= 0.87
Figure 6.7: A contour plot of the maximum absolute di�erence values of � from the
equilibrium value �0 (max j���0j) attained along closed-loop system trajectories
initialized at di�erent (�;) and at R = 0:12 in the state space.
NI which produce closed-loop trajectories inside a given box N of the speci�ed
equilibrium � at �0 = 1:2. If a performance criterion is to keep the system state
within a prespeci�ed region around the equilibrium, then one can determine the
set of initial values NI for which this constraint is met with implementation of the
closed-loop controller. In other words, we have constructed a graphical display of
the operating region of the compressor with implementation of the optimal closed-
loop controller developed in this paper.
VI.B.8 Timing and Scaling to Higher Dimensions
The numerical methods described here can easily solve optimal control
problems in higher dimensions. The run-times necessary to calculate optimal tra-
jectories grow with increased dimension, yet their growth is slow enough such that it
generally does not become a limiting factor. The \curse of dimensionality" however
120
1 1.02 1.04 1.06 1.08 1.1 1.12 1.14 1.16 1.18 1.21.9
1.95
2
2.05
2.1
2.15
2.2
Phi
Psi
R <= 0.88
R <= 0.58
R <= 0.12
Figure 6.8: A contour plot of the maximum values of R attained along closed-loop
system trajectories initialized at di�erent (�;) and at R = 0:12 in the state space.
becomes apparent when one attempts to \map out" the state space by iteratively
calculating optimal trajectories. For example, assume one were to use a higher
order model of the jet engine compressor with dimension 4 and calculate optimal
trajectories over a grid of initial conditions in four of the state variables. Even a
rough grid of eight di�erent values for each state variable would require the calcu-
lation of 84 = 4096 trajectories. At 10 sec/traj (SPARC IPX), this would require
more than 11 hours of calculation plus storage considerations.
A more intelligent selection of trajectories to be calculated would exploit
the principle of optimality which tells us that intermediate points reached along a
trajectory also represent initial conditions for optimal trajectories. Consequently,
the value function is automatically calculated along the entire trajectory. For exam-
ple in R4, one may need only calculate 83 = 512 trajectories. Likewise in dimension
5, a cautious estimate on run times is 20 sec/traj on a SPARC IPX for 84 trajecto-
ries which yields about 22 hours of calculation. Thus, one can avoid much excess
121
calculation by choosing the trajectories to more e�ciently map out the state space.
VI.C Notes
The text of Chapter VI, in part, is a reprint of the material as it will appear
in IEEE Control Systems Technology under the title \Numerical Solution of Non-
linear H2 and H1Control Problems with Application to Jet Engine Compressors,"
and with authors, Michael Hardt, J. WilliamHelton, and Kenneth Kreutz-Delgado.
The dissertation author was the primary researcher and author and the co-authors
listed in this publication directed and supervised the research which forms the basis
for this chapter.
Chapter VII
Case Study: Minimum Energy
Biped Walking
VII.A Introduction
Walking is a characteristic of animals which takes them from one geo-
graphic position to another. There are many variations of walking such as moving
at di�erent speeds, starting, stopping, changing direction, and moving up and down
di�erent grades of slopes. All walking motion consists of a rhythmic displacement
of body parts to maintain a forward motion. If the action is unchanging, then the
motion will generally become periodic. People have a particularly unstable con�g-
uration as they walk with only two legs. After human walking has been learned,
it seems to be such a simple process, yet it remains as one of the principle open
research problems in robotics due its complexity and high dimension, particularly
for gracile systems having long limbs and small feet.
Our goal in this chapter will be to understand better the mathematical
modeling of the human walking motion. We will create a multibody system model
of a biped walker and calculate its motion using its dynamical properties. As in
any control problem, the input applied forces which stimulate motion in the biped
(much as muscles in a person cause a person to walk) are initially unknown. By
122
123
selectively choosing the correct performance objectives for a walking system, we
may solve for these applied forces as the solution to an optimal control problem.
Since it has been shown by biologists [79, 89] that people naturally walk in an
energy e�cient manner, we make the choice to minimize the injected energy into
the system as a function of the input applied forces. Our solution of the minimum
energy biped walking problem produces a very natural walking motion. It is also
the �rst reported results using a model nearly as complex and close to a human
model as the one we use. Preliminary results for the soluton of this problem were
�rst presented in [31].
Many researchers have studied the problem of controlling a walking biped
robot and the associated path planning problem of generating optimal periodic
trajectories for the walking motion. Several of the past innovations and recent
contributions are found in the references [17, 16, 27, 25, 46, 64, 80, 81, 85]. Due
to the complexity of the problem, compromising simplifying modeling assumptions
were often made to make it more tractable. Consider a simple 5-link biped robot
with all rotational joints and full motion degrees of freedom. It will have a 14
dimensional state space when represented with respect to generalized coordinates.
This is in addition to several general nonlinear constraints which \turn-on" in a
state-dependent and possibly asynchronous manner. For example, when a foot is
in contact with the ground, this produces algebraic constraints. The result is a
time-varying di�erential algebraic system.
During walking, there are naturally two phases of a step or what is also
commonly called a gait cycle.1 A gait cycle consists of an interval of time during
which a periodic succession of events occurs. The �rst phase has one foot in contact
supporting the body while the other swings, while in the shorter second phase both
feet are in contact with the ground. During both phases we have a di�erential-
algebraic system, where in the second phase the number of algebraic constraints will
1We will not consider gaits associated with running and can ignore the possibility of a third ight
phase.
124
be greater. Additional considerations occur in between phases when the swing foot
collides with the ground which translates to jump discontinuities on the velocities.
Other algebraic constraints are in the form of saturation constraints on the state
variables and the applied forces. We explain how our numerical procedure is able
to produce solutions which can satisfy all of these constraints.
Some of the early work done by Kajita had a surprising amount of success
by modeling the biped dynamics as that of an inverted pendulum with point masses
[46]. The idea of searching for a passive walking motion which can approximate a
motion which requires only the minimal energy was expressed in the work of McGeer
[64] and later with Goswami et al. [27]. The minimal energy path is desirable for it
exhibits stabilizing, attractive properties. Our experiments have shown that many
walking trajectories naively chosen to approximate walking motion can require a
huge increase in the needed energy over that of the optimally calculated minimal
energy trajectories. Other work also investigating minimal energy biped walking
motion, though with simpli�ed dynamical models, may be found in [80, 81].
The dynamics for the biped are calculated using the recursive, symbolic
models presented in Chapters II, III, and IV. Our dynamical model explicitly ac-
counts for contact forces with the ground and checks to make sure these forces
remain active. Additionally, an inelastic collision is modeled as the swing foot hits
the ground which results in a sudden change in the generalized velocities and a
drop in kinetic energy. As a result, the Contact and Collision Algorithms presented
in Chapter III play an important part in the dynamical calculations. The numer-
ical di�culties normally encountered with di�erential algebraic systems are also
sidestepped with the implementation of the Reduced Dynamics Algorithm.
Numerical results are presented here from our extensive experiments on
various aspects of minimum energy walking. We have investigated the e�ects of
introducing input ankle torques at the point of contact of the legs with the ground
as well as the underactuated case when these torques are not added. Also, an
additional impulsive force can be added at the point in time that the swing leg
125
leaves the ground to aid the body lift o� of the ground, create a smoother motion,
and simulate the human's use of the foot to create a lifto� force. Several other
parameters which can be varied in our model are the biped's step length, the time
of one step, and the proportion of time corresponding to the contact phase. We
discuss the e�ect of these parameters on the system energy.
VII.B Human Walking
The human walking step is composed of two di�erent phases, each charac-
terized by a di�erent set of equations of motion. The �rst phase is the swing phase
or single support phase when one foot is on the ground while the other swings. This
phase makes up the majority of the duration of the walking step in human walking
{ between 80% and 85%. The second phase is called the double support phase as
both feet are on the ground while the body is moving forward. This phase usually
then makes up only 15 � 20% of the human walking step.
Figure 7.1: Walking Phases
126
Also of interest are the transitions between phases. Immediately at the
beginning of the swing phase is the moment of lifto�. Here, the foot is just pro-
pelling the body forward so that the leg loses contact with the ground. The other
transition between the swing and double support phases is characterized by a col-
lision of the swing foot with the ground. Figure 7.1 gives a graphical depiction of
our biped model �rst in the swing phase, then in the double support phase.
To avoid any confusion, some more detailed de�nitions can be useful. The
cadence is de�ned as the number of steps in a standard time frame (e.g. steps/min).
The step length is the distance between the same point on each foot during the dou-
ble support phase. The stride length, on the other hand, is the distance traveled
between two successive foot strikes of the same foot and is equal to two step lengths.
Each stride is, thus, composed of one right and one left step length. All measure-
ments given will be in meters.
Experiments have shown humans to walk in an energy e�cient manner. In
[79], a detailed study is presented of energy expenditure in actual human walking,
where researchers have explored the relationships which exist between real energy
data and human walking motion. A simple yet fairly accurate relationship is one
in which the energy expended is quadratic in the forward velocity,
Ew = 32 + 0:005v2 : (7.1)
Here v is the average forward velocity in m/min and Em is the energy requirement
in cal/kg/min for the average human subject. An often more desirable set of units
for walking energy is to measure it per distance traveled (m = meters) rather than
per time elapsed (min = minutes) since this conveys more the notion of energy
economy. We denote this form of energy as Em. Its units are cal/kg/m, and it is
related to Ew and the previous relationship by
Em =Ew
v=
32
v+ 0:005v : (7.2)
This function will now have a hyperbolic shape. This and most other functional
relationships such as (7.2) indicate a minimum energy motion of an 80 m/min
127
walking velocity with an energy expenditure of 0.8 cal/kg/m. The experiments also
show average cadences of 105 steps/min and an average step length of 0.75 m for
an adult male.
VII.C Biped Model
We believe we are able to capture many of the essential characteristics
of the human walking motion with a 5-link planar biped which walks in the two-
dimensional sagittal plane, the vertical plane bisecting the front of the biped. The
model contains two links for each leg plus a large, massive torso which also func-
tions as the base of the tree-structured multibody system. Though the motion is
constrained to the 2-dimensional vertical sagittal plane, the links are modeled with
a 3-dimensional elliptical shape and a uniform distribution of mass. The physical
data corresponding to the model which we will use as a basis for our experiments
can be found in Table 7.1.
Table 7.1: Biped Model Physical DataLink Mass Length Radius
Torso 20 kg 0.72 m 0.12 mUpper Leg 7 kg 0.50 m 0.07 mLower Leg 4 kg 0.50 m 0.05 m
The in uence of the feet may be modeled in ways which do not increase
the dimension of the system. The two main contributions of the feet to the control
of the biped, when not expressly considering friction, are the introduction of ankle
torques and the lifto� force produced by the heel coming o� the ground together
with the foot rolling over the ball of the foot. It is possible to include ankle torques
in the model by treating these as external forces in uencing the tips of each leg at
the points of contact as described in Section III.E. Rather than modeling a lifto�
force which lasts the entire duration of the double contact phase as is the case with
the foot, we model the lifto� force as an instananeous impulsive force occurring at
128
the moment of lifto�. This last technique has certain numerical advantages though
it cannot completely reproduce the e�ect of the foot as will be shown in the reports
of our numerical experiments.
There exists four points of actuation in the biped model: one applied
torque at each hip and one at each knee. If ankle torques are additionally added,
then there will be two more applied torques, one at the point of contact of each leg
with the ground. The bounds on the ankle torques are generally smaller than those
of the joints as the ankles cannot provide as great a force as at the hips and knees.
The state consists of fourteen states, seven position variables and seven velocity
position variables. As the motion is completely modeled in the two-dimensional
sagittal plane, the torso's position is described by one orientation angle relative to
the inertial frame plus an x and y position coordinate represented in the torso's
local coordinate system. The remaining position coordinates are the angles of the
upper and lower legs relative to their predecessor links. The description of the state
and control variables are as follows:
x1 torso orientation angle relative to the ground.
x2 x-position of the torso hip (axis which connects the joints of both legs) relative
to the origin of the inertial frame in torso local coordinates
x3 y-position of the torso hip relative to the origin of the inertial frame in torso
local coordinates
x4 torso orientation angle velocity
x5 x-velocity of torso hip in torso local coordinates
x6 y-velocity of torso hip in torso local coordinates
x7 hip angle of leg 1 relative to torso
x8 hip angle velocity of leg 1 relative to torso
x9 knee angle of leg 1
x10 knee angle velocity of leg 1
129
x11 hip angle of leg 2 relative to torso
x12 hip angle velocity of leg 2 relative to torso
x13 knee angle of leg 2
x14 knee angle velocity of leg 2
u1 applied torque at hip of leg 1
u2 applied torque at knee of leg 1
u3 applied torque at hip of leg 2
u4 applied torque at knee of leg 2
u5 applied torque at ankle of leg 1
u6 applied torque at ankle of leg 2
VII.D Biped Dynamics
The state equations of the biped walker are of the same form as that of
the forward dynamics of a multibody system experiencing contact forces.
�� =M�1(u+ JTcfc � C � G) : (7.3)
In equation (7.3), M is the square, positive-de�nite mass-inertia matrix, C is the
vector of Coriolis and centrifugal forces, G is a vector of gravitational forces, u
are the applied torques at the links, Jc is the constraint Jacobian, and fc is the
constraint force.
Our 5-link biped model has 7 degrees of freedom (and therefore a 14-
dimensional state space) when not under any constraints. Already in this case,
recursive, symbolic dynamical models are more e�cient for calculating the forward
dynamics than other non-recursive procedures which require constructing and in-
verting the entire mass-inertia matrix,M. O(N ) recursive algorithms, where N is
the number of degrees of freedom of the system, are advantageous for superior cal-
culational e�ciency with increasing degrees of freedom. Due to the symbolic nature
130
of the algorithm, another of the bene�ts of this modeling approach is that changes
made to the kinematic or dynamical parameters simply translate to a di�erent ini-
tialization of the algorithm. No extra calculations are required when parameter
changes are made.
VII.D.1 Dynamics with Contact and Collision
As we are dealing with a tree-structured system, we evaluate the forward
dynamics using the algorithm presented in Figure 2.2. The contact forces experi-
enced at the tips of the legs are calculated with the Contact Algorithm from Chap-
ter III. Similarly, the Collision Algorithm from Chapter III determines the impulse
force experienced at the moment of collision which produces a sudden change in
the joint velocities of the system.
In our numerical trials, we will solve for symmetric, periodic gaits which
will allow us to study just the window consisting of one step. We can then restrict
our attention to just one swing phase followed by one double support phase. The
step begins in phase 1 with leg 2 just leaving the ground to begin the swing phase
and �nishes at the end of phase 2 with leg 1 �nishing the double support phase
in the same relative initial position as leg 2 when the step began. In phase 1,
we need only account for one contact force vector for the ground acting on the
support leg while in phase 2, one contact force vector needs to be calculated for
each leg. A contact force vector will have two nonzero components in the x and y
directions corresponding to the two constraints that a foot not move in either the
vertical or horizontal directions. The �nal joint accelerations will result in a zero
linear acceleration in the vertical and horizontal directions for each contact leg tip.
Figures 7.2 and 7.3 displays the beginning and end con�gurations in addition to
the contact force vectors experienced during the two phases.
Between phase 1 and phase 2, the swing leg makes contact with the ground
and a collision occurs. Not only is an impulse force calculated at the collison leg,
but also one must be calculated at the leg which was already in contact at the time
131
Leg 1
fc1,x
fc1,y
fimp
Leg 2
Figure 7.2: Beginning of phase 1. An
impulse force fimp helps to propel the
body forward. The contact force fc1
is applied to the tip of leg 1.
Leg 2
fc2,x
fc2,yf
c1,x
fc1,y
Leg 1
Figure 7.3: End of phase 2. The con-
tact forces fc1 and fc2 enforce the con-
tact constraints for legs 1 and 2 during
phase 2.
of collision. If one were not introduced at this other leg, then the force of collision
would cause this leg to have a sudden nonzero tip velocity. Similar to the contact
forces, the sudden jump in joint velocities from collision will result in zero linear
velocities for both leg tips in the vertical and horizontal directions.
An impulsive force may not only be introduced to model the e�ects of
inelastic collisions, but it may also be used as a form of control to propel the system
forward. We can use the same Collision Algorithm to introduce an impulsive force
132
during the walking motion to simulate the e�ect of a foot giving the body a lifto�
force to initiate the swing phase. This force is introduced at the beginning of phase
1 at the moment of takeo�. The control impulsive force vector has only one nonzero
component since its direction is restricted to lie along the major axis of the lower
leg. Figure 7.2 displays the direction of the impulse lifto� force vector. In this way,
the impulsive force serves as a thruster to aid the body gain enough velocity to
reach the point where it can begin to fall forward again. This function in humans is
served by the heel of the foot raising o� of the ground while the foot rotates around
the ball of the foot.
VII.D.2 Reduced Dynamics
A key component of our dynamical modeling is the use of the Reduced
Dynamics Algorithm presented in Chapter III. We have already mentioned that
because of the contact constraints we are faced with a di�erential-algebraic system.
Two courses of actions are possible when it is necessary to integrate the dynamics,
one being the use of specially tailored integration routines which often require the
partial derivatives of the various contact constraints. The preferable approach,
however, is to use a reduced unconstrained set of dynamics which evolve on the
constraint manifold. Then it is possible to use standard integration procedures.
Essentially, the holonomic contact constraints in our model allow the sys-
tem to be described by another set of unconstrained dynamics of reduced dimen-
sion. Because they are unconstrained, integration of the reduced dynamics produces
much less numerical di�culties. This algorithm allows us to evaluate the reduced
dynamics in a very e�cient manner. There then exists a direct correspondence to
the full set of dynamics by which one can obtain the full state from the reduced
state.
In the �rst single contact phase of the biped motion, the contact constraint
reduces the total degrees of freedom from 7 to 5. Thus, using the Reduced Dynamics
Algorithm, an unconstrained 10-dimensional state space can represent the system
133
during this period instead of the full 14 dimensions plus algebraic constraints. The
remaining 4 dependent states and their time derivatives can be determined from
the 10 independent states and their time derivatives. The independent states are
those corresponding to the torso and to leg 2, namely xi, i = 1; : : : ; 6; 11; : : : ; 14.
Recall that one of the primary di�culties of the Reduced Dynamics Al-
gorithms is that the inverse kinematics must be used to solve for the dependent
states. For the biped, this is easy since our problem is equivalent to solving for
the joint angles of a 2-link manipulator when its endpoints are known. The follow-
ing well-known solution comes from Spong and Vidyasagar [87]. In Figure 7.4 is
displayed the inverse kinematics problem where �1 and �2 are the desired angles,
a1 and a2 are the lengths of the upper and lower legs respectively, and �1 and �2
are two intermediate angles. We assume that one end of the 2-link arm has been
transferred to the origin while the other end has coordinates (x; y). From the Law
of Cosines,
D = cos(�2) =x2 + y2 � a21 � a22
2a1a2: (7.4)
Also, sin(�2) = �p1�D2. In our case, since the knees only bend in one direction,
the minus sign is always used so that
�2 = tan�1�p1 �D2
D: (7.5)
It is apparent that �1 may be obtained easily from �1 and �2. The angle �1 =
tan�1(y=x) while �2 = tan�1�
a2 sin �2a1+a2 cos �2
�. The �nal expression for �1 is
�1 = tan�1(y=x)� tan�1
a2 sin �2
a1 + a2 cos �2
!: (7.6)
The dependent position states then are x7 = �1 � x1 and x9 = �2.
Also, the velocities are known at both ends of the 2-link manipulator.
The Collision Algorithm can then be used to give the unique joint velocities which
satisfy the known velocity constraints. This is done by initializing the algorithm
for a 2-link manipulator with the known spatial velocity at the joint connecting
the upper leg and the torso. The joint angles have been previously calculated and
134
(0,0)
(x,y)
θ
α
α2
1
1
2
1
a
a
−θ2
Figure 7.4: Inverse Kinematics Problem for 2-link Leg
the joint velocities are arbitrary as there will only be one solution. The updated
velocities will then corroespond to the values of the states x8 and x10.
In the second double control phase, the addition of contact constraints on
the other leg has the e�ect of further reducing the degrees of freedom so that we
can work with a system with only a 6-dimensional state space. The independent
states will be only the six states corresponding to the torso. The joint angles for
each leg may be derived from the above-mentioned inverse kinematics procedure.
The Collision Algorithm also gives the joint angle velocities for each leg as was the
case in phase 1.
In summary, if �1 and �2 equals the independent position and velocity
state variables during phase 1, and �1 and �2 similarly represent the independent
variables in phase 2, then the reduced dynamical equations for both phases of the
135
walking step are
Phase 1 Phase 2
_�1 = �2 _�1 = �2
_�2 = f1(�1; �2; u) _�2 = f2(�1; �2; u)
�1 = [x1; x2; x3; x11; x13]T �1 = [x1; x2; x3]
T
�2 = [x4; x5; x6; x12; x14]T �2 = [x4; x5; x6]
T
(7.7)
where u 2 R6 when ankle torques are included in the biped model and u 2 R4
in a model without ankle torques. The dynamical expressions for fi represent the
reduced dynamics which are of lower dimension than equation (7.3) and account
for the contact constraints.
f1(�1; �2; u1) = M�1�(u�;1 � C� � G�)
f2(�1; �2; u2) = M�1�(u�;1 � C� � G�)
(7.8)
The Reduced Dynamics algorithm evaluates _�2 and _�2 using the forward dynamics
algorithm and the Contact Algorithm.
VII.E Minimum Energy Performance
It is generally accepted that humans follow closely an energy e�cient mo-
tion strategy [79]. In accomplishing our goal of approximating the human walking
behavior, it is natural to also have these same performance objectives for our biped
model. Our problem is a control problem as we need to establish the values for the
applied joint forces which will cause the system to move in an energy e�cient man-
ner. To minimize the injected energy into the system, we choose as our performance
the integral of the applied joint forces,
J =ZT
0uTu dt =
ZT1
0uT1 u1 dt+
ZT
T1
uT2 u2 dt ; (7.9)
where T1 is the time at the end of the �rst phase, and T is the time at the end of the
second phase. The vector u1 are the applied forces at time t during phase 1 while u2
136
are the applied forces during phase 2. The above quantity is not a measure of the
mechanical work performed on the system, nor are we able to determine the change
in energy of the body from its value. This is rather a measure of the required cost
in energy to move the system. The change in energy may re ect the work done
by the electric motors which actuate the various joints of the biped. For a simple
actuation model (as for a commonly used direct drive DC motor), its units will
also be in Joules (J), the units of energy, up to a system-dependent proportionality
constant. This approach provides a more numerically tractable way of reaching our
performance objectives.
If an impulsive force is introduced at the moment of lifto�, uimp;1, this
force should also be included in the performance. This is an instantaneous force
which enters the performance outside of the integral.
J = uTimp;1uimp;1 +
RT1
0 uT1 u1 dt+RT
T1uT2 u2 dt (7.10)
This general form of minimum energy performance was also used in [81].
Because we are modeling the collision of the foot with the ground as
completely inelastic, the system will experience a sudden drop in the kinetic energy
at the moment of collision. Since the potential energy is not a�ected by the collision,
the total energy will also drop by the same amount. Figure 7.12, which is found in
Section VII.G on our numerical experiments, displays a typical plot of the kinetic,
potential, and total energies all centered around zero of a typical minimum energy
walking step of a biped robot. The kinetic energy is responsible for most of the
trend seen in the total energy as the potential energy varies less. The energy that is
lost at collision must all be injected back into the system over the remainder of the
step with the input torques. By minimizing the integral of the squared magnitude
of the control torques over the step, we are also minimizing the loss of energy
experienced at collision and, thus, working towards conserving as much energy as
possible during the step.
137
VII.F Numerical Optimal Control
Direct optimization methods for optimal control are characterized by the
minimization of a cost functional which is a function of the system state and the con-
trol input u. An example of such a method is the program DIRCOL [94, 95], which
can handle implicit or explicit boundary conditions, arbitrary nonlinear equality
and inequality constraints on the state variables, and multiple phases where each
phase may contain a di�erent set of state equations. DIRCOL functions by pack-
aging the optimal control problem along with its constraints into a constrained,
nonlinear minimization problem which is solved by an SQP-based optimization
code NPSOL (Gill, Murray, Saunders, Wright [26]).
DIRCOL discretizes the state and control variables in time over the tra-
jectory. The �neness or coarseness of the discretization can have a large in uence
over the time required to generate a solution. A new version using sparse opti-
mization techniques should be much faster. The output of the numerical optimal
control program will be the optimal open-loop solution for the control u(t) and
the corresponding state trajectory x(t) at the choice of grid points in time. With
regards to the biped walking control problem, the �nal values of u(t) and x(t) at
the end of a step will be symmetrically constrained to match the initial values for
u and x at the beginning of the step. This produces an optimal periodic solution
for the desired walking trajectory.
We discussed the use of direct minimization techniques in Section V.C of
Chapter V. This numerical approach, and particularly DIRCOL, was selected over
all the others for the ease with which many di�erent kinds of constraints can be
added to the control problem. The numerical procedure is very reliable and also
informative when the program cannot arrive at an optimal solution. DIRCOL has
the capability to de�ne unknown constant parameters which may be allowed to vary
within a speci�ed range. These parameters may be used to additionally optimize
over the step length, �nal time, and/or forward velocity.
138
Some examples of the very nonlinear constraints that the �nal solution
must satisfy and which must be inputted into DIRCOL are that the foot land at
the speci�ed step length when this is �xed or at the value set for the parameter
when it is variable. The vertical component of the contact forces experienced by
the feet during each of the phases must also remain positive, otherwise the feet will
leave the ground and a di�erent set of equations will govern the system dynamics.
Finally, the vertical component of the contact force of the foot that is about to leave
the ground must reach zero at the end of phase 2 in preparation for lifto�. We now
outline more explicitly the various constraints of the optimization problem.
VII.F.1 Box Constraints and Polar Position Coordinates
The simplest constraints imposed on the problem are the box constraints.
These are magnitude constraints on the state, control, and parameter variables. It
is always preferable to choose these form of constraints whenever possible as they
are much more numerically tractable than general inequality constraints which are
nonlinear functions of the states, controls, and parameters. Through a change of
coordinates it is sometimes possible to reformulate a nonlinear inequality constraint
as a simple box constraint with a di�erent set of variables. This simple trick we
utilize in the biped control problem.
Recall that there exist two position variables describing the x and y po-
sition coordinates for the torso and, consequently, the entire biped robot. These
position coordinates are represented in the local torso coordinate system. One of
the nonlinear inequality constraints which we would normally be forced to impose
is that the hip remain at a distance from the origin no greater than the length of
an extended leg. This requirement a�ects leg 1 which supports the body during the
swing phase. It is important since with the use of the Reduced Dynamics Algorithm
the position and velocity variables for leg 1 are not part of the state used in the
optimization process. Their values must be calculated via inverse kinematics and
the Collision Algorithm every time the dynamics need to be evaluated. If the hip
139
is too far from the origin, then we will not have su�cient information to determine
the state of leg 1 plus the system will have entered a free- ying con�guration during
phase 1 or a single-support con�guration during the double support phase.
By converting these Cartesian coordinates to polar coordinates r and �,
it is then possible to place a simple magnitude constraint on r which will serve the
same function as the nonlinear inequality constraint previously mentioned. De�ne
the following variables
x02 = r =qx22 + x23
x03 = � = tan�1(x3=x2)
x05 = (x2x5 + x3x6)=x02
x06 = (x2x6 � x5x3)=x022
(7.11)
where x05 and x06 are the respective inertial time derivatives of x02 and x03. These
alternative states will replace x2, x3, x5, and x6 in the numerical optimal con-
trol program. We need to use linear Cartesian coordinates when evaluating the
dynamics so that before this is done we must make the substitutions
x2 = x02 cos x03
x3 = x02 sinx03
x5 = x05 cos x03 � x02x
06 sinx
03
x6 = x05 sinx03 + x02x
06 cos x
03
(7.12)
After evaluating the dynamics and before passing the time derivative of the state
to the integration routine, we again make the substitutions
_x02 = x05
_x03 = x06
_x05 = (x2 _x5 + x25 + x3 _x6 + x26)=x02 � (x2x5 + x3x6)x
05=x
023
_x06 = (x2 _x6 � x3 _x5)=x022 � 2(x2x6 � x5x3)x
05=x
033
(7.13)
During the optimization trials to be discussed later, we typically place the
140
following box constraints on the state and control variables:
1:35 � x1 � 1:60
0:80 � x02 � 0:998999
�3:00 � x03 � 3:00
�25:00 � x4 � 25:00
�25:00 � x05 � 25:00
�25:00 � x06 � 25:00
�4:00 � x11 � �2:60
�25:00 � x12 � 25:00
�1:00 � x13 � �0:0895
�25:00 � x14 � 25:00
0:00 � x15 � 108
(7.14)
�30:00 � u1 � 30:00
�30:00 � u2 � 30:00
�30:00 � u3 � 30:00
�30:00 � u4 � 30:00
�15:00 � u5 � 15:00
�15:00 � u6 � 15:00
(7.15)
These box constraints will be the same in both phases with the exception of u6
which refers to the ankle torque of the swinging leg in phase 1. Thus, we replace
its box constraint with
0:00 � u6 � 0:00 (7.16)
during phase 1.
The additional state variable x15 refers to the value of the integral (7.9).
For numerical reasons, the Mayer objective functional approach is taken where the
optimization problem is to minimize x15+uT
imp;1uimp;1 with _x15 being the integrand
of (7.9).
141
VII.F.2 Inequality Constraints and Boundary Conditions
We give the boundary conditions for one step. For the beginning and
�nal time, if R(x1) is the rotation matrix mapping inertial coordinates to torso
coordinates and step is the length of the step taken, let hp = [hp1 hp2 hp3]T =
R(x1)[step 0 0]T . This quantity is the displacement of the hip in torso coordinates.
Then, the explicit initial and �nal time boundary conditions are:
x+1 = x�1 u+1 = u�3
x+2 = x�2 + hp1 u+2 = u�4
x+3 = x�3 + hp2 u+3 = u�1
x+4 = x�4 u+4 = u�2
x+5 = x�5 u+5 = 0
x+6 = x�6 u+6 = u�5
(7.17)
where the variables with a + are the values at the �nal time and those with a �
at the initial time. The remaining state variables do not need to be speci�ed as
the boundary conditions occur at a moment of double contact of the feet with the
ground. The dependent positions and velocities can be determined from the �rst
six states.
It was mentioned earlier that it is possible to include in our model an
impulsive lifto� force at the beginning of phase 1. When this is done, all the
joint velocities in the system will undergo a discontinuous jump as a result of the
impulsive action. The jump in the velocities may be calculated as a function of the
previous state y and a lifto� impulsive force vector uimp;1 as described in Section
III.E. De�ne the function x� = IMP(y; uimp;1) which calculates the state vector
x� after introducing an impulsive force at the beginning of phase 1 to leg 2. The
relationship y = IMP�1(x�; uimp;1) = IMP(x�;�uimp;1) holds which allows us to
calculate the right handed limits for the torso velocities:
x+4 = y4
x+5 = y5 (7.18)
142
x+6 = y6
These boundary conditions replace those for x4, x5, and x6 in (7.17).
The boundary conditions in between phases enforce continuity of the po-
sition state and control variables, and they prescribe the jump in the velocity state
variables due to the collision. The function xc = COLL(x) gives the states after
collision after determining the required jump in the velocities from the Collision
Algorithm. xc is a state vector of the same dimension as x. Then the explicit
boundary conditions in between phase 1 and 2 are:
u+i
= u�i
i = 1; : : : ; 6
x+i
= x�i
i = 1; : : : ; 3
x+i
= xci i = 4; : : : ; 6
(7.19)
where here the + indicates the right-hand side limit, at the beginning of phase 2,
and the � indicates the left-hand side limit at the end of phase 1.
Implicit boundary conditions are characterized by nonlinear equations to
be satis�ed by the boundary values for the states, controls, and parameters. The
required implicit boundary conditions that we impose on the problem at the initial
and �nal time are that the initial tip position of leg 2 start at the appropriate step
length away from leg 1 and with an initial zero tip velocity if no impulsive lifto�
control force uimp;1 is added. If the impulsive force is added, then the starting
velocity will depend on the magnitude of the impulsive force. Let the function
[tp; tv] = TIP(x�) evaluate the current value for the leg 2 (swing leg about to
lifto�) tip position and velocity in inertial coordinates at the beginning of phase 1.
Both tp and tv are 2-dimensional vectors. Additionally, we must require that the
vertical component of the contact force in uencing leg 1 be zero at the end of phase
2 so that the leg is ready to leave the ground at the beginning of phase 1. Let fc =
CONT (x+; u+) be the 6-dimensional spatial contact force in inertial coordinates for
leg 1 at the end of phase 2. Then the nonlinear, implicit boundary conditions
143
at the initial and �nal time are:
r1 = tp1 + step = 0
r2 = tp2 = 0
r3 = tv1 = 0
r4 = tv2 = 0
r5 = fc5 = 0
(7.20)
Note that the vector fc is a spatial vector in which the �rst three components are
the moment forces and the next three components are the linear forces. In our
case, we are concerned with the linear, vertical component; thus, we require its
�fth entry to be zero.
Now, we consider the alternative case when lifto� impulsive forces are
introduced at the very beginning of phase 1. In this case, we remove the e�ect
of the impulsive lifto� force from x� by using the previously described function
y = IMP�1(x�; uimp;1) = IMP(x�;�uimp;1). Then [tp; tv] = TIP(y) gives the tip
position and velocity vector for y. The positions are unchanged from the impulsive
force, and the tip velocities for the state y should be 0 when the e�ect of the
impulsive force has been removed from x�. The boundary conditions then remain
the same as in (7.20).
We additionally require that the swing leg land at the appropriate step
length at the moment of collision. Let [tp; tv] = TIP(x�) be the tip position
and velocity at the end of phase 1. The implicit boundary conditions for in
between phases are:
s1 = tp1 � step = 0
s1 = tp2 = 0(7.21)
As mentioned previously, it is possible to make additional parameters
variable such as step length, the time duration of the walking step, and average
144
forward velocity. De�ne the following parameters:
p1 = step length
p2 = magnitude of lifto� impulsive force
p3 = time of collision
p4 = average forward velocity
p5 = proportion of step time corresponding to phase 1
(7.22)
Let T be the �nal time or time duration of the walking step while T1 = p3. The value
for T is also allowed to be variable though it is not necessary to give it a parameter
de�nition. The relative time of the step during which phase 1 occurs is related to
T in the following manner, p5 = T1=T . This parameter will generally be �xed for
the majority of our experiments and set to 0:85 for reasons to be explained later.
The previously mentioned problem would then introduce the following additional
implicit boundary conditions for the initial and �nal times:
r6 = T � p3=p5 = 0
r7 = p4 � (p1 � 60)=T = 0(7.23)
and additional implicit boundary conditions for in between phases:
s3 = p3 � T1 = 0 (7.24)
Note that if the proportion of total step time for phase 1, p5, is desired to be �xed,
we may set the upper and lower bounds for this parameter to be the same value.
In addition to boundary conditions, we also have nonlinear inequality
constraints which must be satis�ed during the entire length of either phase. The
vertical components of the contact forces experienced by the feet in contact during
both phases must be positive as a negative value signi�es that the foot is leaving
the ground. Also, the swing leg must stay entirely above ground during phase 1.
Thus, the nonlinear inequality constraints for both phases are:
Phase 1: fc5(Leg 1) � 0
145
tp2(Leg 2) � 0
Phase 2: fc5(Leg 1) � 0
fc5(Leg 2) � 0
where fc = CONT (x; u) and [tp; tv] = TIP(x). These constraints prevent a prema-
ture lifto� of a foot from the ground when it should be, for example, in the double
support phase.
The functions used in the constraints, IMP(x), COLL(x), TIP(x), and
CONT (x; u), all have spatially, recursive implementations for their evaluation.
VII.G Optimization Trials
The high degree of nonlinearity and high dimension of the problem along
with all the constraints make it unreasonable to assume that by specifying the
state equations, boundary conditions, and inequality constraints along with a naive
initial guess of the solution, that the optimization procedure will immediately �nd
an optimal solution. Optimization procedures are too sensitive to initial guesses.
Consequently, an iterated process was undertaken which gradually approximated
the actual problem. The �rst problem solved was merely �nding the minimal energy
solution to standing in place. Using that solution as an initial guess, it was possible
to solve for the problem where the biped was forced to move a small distance. This
iterative process continued several times with increasing distances over which the
biped traveled.
During the iterative process, constraints were also gradually added such as
the double support phase and the collision e�ect. After about 10 such optimization
runs, it was possible to un�x the initial and �nal boundary conditions for the state
and controls and merely require that the solution be periodic. In this manner, the
complete problem was solved.
For most trial runs, we used 13 grid points in time, 8 in the �rst phase
and 5 in the second phase. As the number of grid points has a large in uence
146
on the length of each optimization run, it is preferable to use a coarse grid for
trial runs, then to re�ne the grid when very exact solutions are desired. Sample
run times varied between 25 and 35 minutes on a Sun Ultra 1 depending upon
the complexity of the problem. Failure of the program to converge generally was
dependent upon inherent characteristics of the model or of the problem and not of
DIRCOL's inability to deal with the nonlinearity and high dimensionality. These
occurrences usually led to a rethinking of the model and/or a deeper understanding
of the problem.
Many di�erent numerical experiments were made as there are many pa-
rameters which can be �xed or varied, and there are modeling exibilities. Two
main categorizations can be made in that we explore �rst walking without any form
of lift propulsion. We then add to our biped the possibility of introducing an in-
stantaneous impulsive force at the moment of lifto� to help the body move forward.
This lifto� impulsive force also serves to better simulate the function of the feet
which are only implied in our model but do not exist explicitly. The impulsive force
also has its cost and is added into the performance. In both settings, the additional
e�ect of the removal of ankle torques is investigated.
The only parameter �xed in our experiments is the percentage of time of
the total step that the walker is in the double support phase. We set this parameter
to be 15% as this is comparable to observed human data. It is possible with our
numerical approach to also optimize over this quantity, and our results shows that
a lower energy walking trajectory can be obtained by reducing this proportion. The
reason for this e�ect is that without having the use of the feet in the model, there
is little bene�t (energywise) to having a double support phase. Impulsive lifto�
forces do not counteract this e�ect as they are instantaneous. The double support
phase is desirable for it essentially provides a smoother walk which resembles the
human motion. A negative consequence, also, of reducing the proportion of 15% to
something smaller is that the optimal control torques will then acquire excessively
high control rates.
147
Though in the following subsections, we will explore global minimum en-
ergy walking, there also exist model variations which can be made and which are
interesting for comparative purposes. There are four speci�c cases to be considered
and graphically compared:
Model 1 Walking without an impulsive lifto� force and with ankle actuation.
Model 2 Walking without an impulsive lifto� force and without ankle actuation.
Model 3 Walking with an impulsive lifto� force and with ankle actuation.
Model 4 Walking with an impulsive lifto� force and without ankle actuation.
VII.G.1 Models 1 and 2: Walking without lifto� forces
Most everyone is familiar with the useful function of the feet in walking.
As soon as the double support phase is entered, the heel of the back leg begins to
lift o� of the ground, thereby providing a lifto� force for the rest of the body until
the point where the ball of the foot breaks contact with the ground. One of the
principal di�erences of our biped model with human walking is that the feet are
implicitly, but not explicitly, included in the model. We �rst explore this interesting
case when no lifto� forces besides the hip and knee applied torques are available to
help the back leg lift o� of the ground.
In Figure 7.5 are displayed the input torques for a complete, periodic,
double step. The stick walking �gures on the top portion of the �gure indicate
for the two plots beneath it which part of the walking step the plotted points
correspond to. This is done by examining the point, then its position in the step
is obtained by observing the stick �gure directly above it. The plots begin with
the swing leg leaving the ground. At the �rst vertical line, the swing collides with
the ground, and at the second vertical line, the other leg lifts o� of the ground.
The right-hand side of the plot, however, corresponds to the torques applied to the
support leg. In the middle plot are the applied torques to the system with ankle
148
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−40
−20
0
20
40Applied Torque at Hip
Tor
que
(N−
m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−10
0
10
20
30Applied Torque at Knee
Tor
que
(N−
m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−10
0
10
20Applied Torque at Ankle
Tor
que
(N−
m)
Figure 7.5: Optimal applied torques for walking without lifto� forces. Included
extra forces in addition to hip, knee torques for optimal speed: Ankle (solid),
None (dashed). Included extra forces for 50 m/min speed: Ankle (dashdot), None
(dotted)
actuation.
The solid and dashed lines indicate the torques for the optimal walking
motion for the model with and without ankle actuation respectively when the only
parameter �xed is the proportion of time for the double support phase. This is
�xed at 15%. The paths for these lines are seen to be almost identical signifying
very little overall di�erence for when ankle torque actuation is included.
Also displayed in Figure 7.5 are control trajectories for the optimal walking
motion when the average forward velocity is �xed at 50 m/min, also with (dashdot)
and without (dotted) ankle actuation. The faster 50 m/min walk also shows little
149
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20.92
0.94
0.96
0.98
1Vertical Displacement of Hip
Ver
t. D
isp.
(m
)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20.4
0.45
0.5
0.55
0.6Vertical Displacement of Knee
Ver
t. D
isp.
(m
)
Figure 7.6: Optimal trajectories of hip, knee for walking without lifto� forces.
Included extra forces in addition to hip, knee torques for optimal speed: Ankle
(solid), None (dashed). Included extra forces for 50 m/min speed: Ankle (dashdot),
None (dotted)
di�erence between including ankle actuation and not including it. This e�ect is
universal throughout out experiments. There does exist, however, a signi�cant
di�erence between the slower globally optimal torques which are smoother and of
smaller magnitude. The greater forces display at what times during the step the
walking propulsion forces are most necessary.
In Figures 7.6 and 7.7, the movement of the hip and knee is analyzed. The
height of the hip and knees stays roughly at the same level during the slower globally
optimal walk while during the faster walk, the well-known sinusoidal motion e�ect of
the hip is more apparent [79]. Interesting is also the hip forward velocity displayed
in Figure 7.7. There is a large variation in the forward velocity of the faster walk
150
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.5
0
0.5
1Horizontal Displacement of Hip
Hor
iz. D
isp.
(m
)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−1
0
1
2
3Horizontal Velocity of Hip
Hor
iz. V
el. (
m/s
)
Figure 7.7: Optimal trajectories of hip position and velocity for walking without
lifto� forces. Included extra forces in addition to hip, knee torques for optimal
speed: Ankle (solid), None (dashed). Included extra forces for 50 m/min speed:
Ankle (dashdot), None (dotted)
indicating that the system is not able to maintain a high velocity but must spend
a great deal of energy during each step to reach the higher speed.
VII.G.2 Models 3 and 4: Walking with lifto� forces
In human walking, the body propels itself o� of the ground with the foot
and gains potential energy before it starts to fall forward to the point where the
swing leg collides with the ground. To simulate this e�ect, we add an instantaneous
impulsive force at the moment of lifto�. This force can add the needed upward
velocity to the back leg and torso to more easily reach the point where the body
begins to fall forward. The impulsive force is a linear force and has only one
151
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−40
−20
0
20
40Applied Torque at Hip
Tor
que
(N−
m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−10
0
10
20
30Applied Torque at Knee
Tor
que
(N−
m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−5
0
5
10
15Applied Torque at Ankle
Tor
que
(N−
m)
Figure 7.8: Optimal applied torques for walking with lifto� forces. Included forces
in addition to hip, knee torques and impulsive force for optimal speed: Ankle
(solid), None (dashed). Included forces for 50 m/min speed: Ankle (dashdot),
None (dotted)
nonzero component along the axis parallel to the lower leg about to come o� of the
ground. The cost in terms of energy for introducing this extra control is included
into the performance via (7.10). There exists then a tradeo� between the size of
the impulsive force to use and using more or less torque forces during the walking
step.
Figure 7.8, in comparison with Figure 7.5, provides evidence that the
impulsive force is su�ciently bene�cial to allow a signi�cant savings in the applied
torques required for moving at the optimal 50 m/min walk. The applied torques
are much smoother and of smaller magnitude; particularly the hip torques which
152
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.5
0
0.5
1Horizontal Displacement of Hip
Hor
iz. D
isp.
(m
)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−1
0
1
2
3Horizontal Velocity of Hip
Hor
iz. V
el. (
m/s
)
Figure 7.9: Optimal trajectories of hip position and velocity for walking with lifto�
forces. Included extra forces in addition to hip, knee torques for optimal speed:
Ankle (solid), None (dashed). Included extra forces for 50 m/min speed: Ankle
(dashdot), None (dotted)
are surpringly close to zero for the majority of step. Figure 7.9 also displays how
with the use of the impulsive force the body has a more slowly varying average
forward velocity. In the cases without an impulsive lifto� force, we saw a quick
reduction in the average forward velocity while the body is rising to the peak of the
swing phase. With the use of the impulsive force, however, the body does better
at keeping its forward momentum.
VII.G.3 Varying Step Length, Forward Velocity, and Final Time
It was mentioned earlier how equation (7.2) describes the energy require-
ments of human walking motion in that it has been observed that the required
153
10 20 30 40 50 60 70 800
2
4
6
8
10
12
Ene
rgy
(γ c
al/m
/kg)
Average forward velocity (m/min)
Figure 7.10: Required energy as a function of average forward velocity. Ankle
actuation (solid), no ankle forces (dashed), ankle actuation and impulsive force
(dashdot), impulsive force with no ankle actuation (dotted).
metabolic energy for walking has a hyperbolic relationship with the average for-
ward velocity. For positive velocities, most experiments show an optimum of 80
m/min. In our experiments, we have witnessed the same e�ect as is displayed in
Figure 7.10, though with a very di�erent optimum of approximately 12 m/min.
Interestingly, the optimum was almost the same for the four di�erent models con-
sidered. The value of energy on the vertical axis of Figure 7.10 is obtained by
dividing the performance (7.9) or (7.10) by 4.186 to get calories (up to an actuator-
dependent proportionality constant), by the total mass of 42 kg for our model to
get cal/kg, and by the �nal value for the step length to �nally get cal/kg/m.
A possible conjecture for the disparity with optimal human walking is the
154
10 20 30 40 50 60 70 800
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Ste
p Le
ngth
(m
)
Average forward velocity (m/min)
Figure 7.11: Step length as a function of average forward velocity. Ankle actuation
(solid), no ankle forces (dashed), ankle actuation and impulsive force (dashdot),
impulsive force with no ankle actuation (dotted).
lack of the foot e�ect which provides essentially an extension of the leg when the
back heel lifts o� of the ground propelling the body forward. Walking with the
added impulsive force is seen to provide a signi�cant energy savings, particularly
with higher velocity.
Finally, Figure 7.11 displays how the optimal size of the step increases with
increasing average forward velocity. The overall trend is very close for all the ex-
perimental cases considered in that the optimal step length is directly proportional
to the average forward velocity.
155
0 0.1 0.2 0.3 0.4 0.5−1
−0.5
0
0.5
1
1.5
2Globally optimal walking without impulsive force
0 0.1 0.2 0.3 0.4 0.5−1
0
1
2
3Globally optimal walking with impulsive force
0 0.2 0.4 0.6−20
−10
0
10
20
30
40Optimal 50 m/min walking without impulsive force
0 0.2 0.4 0.6−20
−10
0
10
20
30
40Optimal 50 m/min walking with impulsive force
Figure 7.12: Kinetic (dashed), Potential (dotted), and Total (solid) energies cen-
tered at 0 during a walking step.
VII.G.4 Optimal Forward Velocities and Energy Requirements
In our investigations, we have been minimizing the integral of the squared
applied torques and calling the solution minimum energy walking trajectories. We
should emphasize that these solutions do not produce the minimum change in en-
ergy of the system as we are not minimizing the mechanical work of the system.
In fact, we are minimizing a quantity proportional to the required energy for mo-
tion. In humans, this is analogous to the di�erence between mechanical energy and
metabolic energy. As no system, not even a human, is perfectly e�cient, these quan-
tities will di�er and their relationship in humans still remains a very di�cult and
unanswered problem [79]. In robotics, we do not have metabolic energy, but there
does exist the required energy for direct drive motors at the joints to produce the
156
required torques. Our chosen performance (7.9) is equal, up to a system-dependent
proportionality constant, to the energy used by simple direct drive motors to move
the biped robot.
Figure 7.12 displays the kinetic, potential, and total energies all centered
around zero of several minimum energy walking steps of a biped robot. The sharp
drops in the kinetic and total energies signi�es the moment of collision. Before this
point, the time when the body reaches its maximum height and begins to fall is
indicated by the peak of the potential energy curve. The left plots are without
impulsive lifto� forces, while on the right they are modeled. The top plots are
globally optimal walking steps, and the bottom are 50 m/min optimal walking
steps. In all cases, ankle actuation is modeled.
The faster walking steps exhibit a much larger variation in its energy
than globally optimal walking. The body also stays much longer in positions of
high potential energy. For the faster speeds, the body doesn't slow down as it
reaches the peak of its potential energy before it begins to fall forward. Rather, it
continuously builds its kinetic energy until the moment of collision. Also for the
faster speed, the walking step which uses the lifto� impulsive force (bottom-right)
has evidently the smoothest and homogenous motion with the fewest oscillations.
This particular walking step builds up its kinetic energy, then keeps moving for a
long period while keeping the kinetic energy roughly constant.
Chapter VIII
Conclusions and Future Directions
This work gathers together several di�erent research areas in the modeling
and control of complex, nonlinear systems. As we stated at the outset, the main
goal is to battle the well-known \curse of dimensionality" by providing numerical
and algorithmic tools which enable the user to better understand and control such
systems. In the areas of articulated multibody systems, e�cient, recursive, and
symbolic modeling techniques are further developed and re�ned. Multiple degree-
of-freedom joints are expressly considered and valuable insight is also provided for
the implementation of these methods. Later, important contributions are made for
the class of such systems experiencing contacts and collisions with the environment.
An entire section is devoted to the development of closed-form symbolic expressions
for the derivatives of the dynamics and its components. Dynamical partial and time
derivatives have numerous applications in estimation and control of which some are
described in this dissertation.
A new result is the ability to evaluate recursively and e�ciently the adjoint
equations of a general multibody dynamical system for uses in numerical optimal
control. The possibility to even calculate these equations without undergoing the
laborious task of constructing the closed-form equations is not yet known in the
research community, and it can open up new approaches for solving optimal control
problems. Another important result found in the dynamics chapters is the Reduced
157
158
Dynamics Algorithm which is designed for use with algebraically constrained dy-
namical systems. This algorithm can evaluate the unconstrained reduced dynamics
which account for the constraints using the same suite of recursive, dynamical algo-
rithms. This is done without having to explicitly construct the reduced dynamics
thereby providing an e�cient means for the integration of di�erential-algebraic
multibody systems. This algorithm became vital for the solution of the minimum
energy biped gait generation problem presented in the last case study.
By bringing together various representations of recursive, multibody algo-
rithms, extending the results to more general types of systems, and exploring new
applications for these methods, our hope is to make further progress in bringing
together a comprehensive package that can be used for the dynamics, estimation,
and control of such systems. The exibility and ease with which these symbolic
equations may be manipulated were shown throughout the dissertation. In addi-
tion, its special structure makes many interesting uses of the dynamics possible.
The work begun in [30], for example, on exploiting the special structure found in
the multibody equations in control design is continuing. We discussed therein a
backstepping approach for adaptive control. Work has also begun on using the
recursive expressions for the derivatives of the spatial matrix operators of the dy-
namics to construct a dynamics-based extended Kalman �lter estimation algorithm
of multibody motion in computer vision.
In the context of nonlinear control, numerical techniques for the solution
of nonlinear H2 and H1 control problems are shown to be viable and feasible for
the solution of important practical applications such as the jet engine compressor
control problem. The ability to solve such problems has been vastly underestimated
in the research community. We provide a thorough description of the state-of-
the-art numerical approaches which exist and have been tested on these types of
problems, and we propose a particular solution method. Through illustrations and
the �nal two case studies, we demonstrate in a step-by-step manner methodologies
by which one can approach and solve various nonlinear control problems.
159
In the case of the jet engine compressor control problem, our approach
exposed the large sensitivities to control rate saturation prevalent in this system.
This case study demonstrates a how simple application of the numerical control
techniques is not su�cient to completely analyze a control problem. One must
consider the characteristics of the system under study and often devise special
techniques to handle the di�culties one encounters. Modeling considerations can
then become of primary importance. Our results suggest how a compressor model
with greater actuation possibilities may be needed to e�ectively control the severe
instabilities of surge and rotating stall.
No problem better exempli�es the complexity of the control of a complex,
nonlinear system as does that of our �nal investigation into biped walking. It lies
on the frontiers of nonlinear control with modeling complexities such as contact
and collision constraints, di�erential-algebraic dynamical equations, high dimen-
sion, periodic control, and control and rate saturation constraints. In spite of these
obstacles, we have been able to use the extensive structure brought together here
to solve this problem. Previous research has not solved the problem of minimum
energy gait generation with as complete a dynamical model as ours.
There exists many future avenues for expanding our work on the problem
of biped walking control since with the structure and framework we have created,
it now becomes relatively easy to add new features to our model. Our �rst step
to gain more understanding into biped walking will be to explicitly model the feet
to see which e�ect this has on our global minimum energy walking results. It is
believed that with their ability to extend the legs during the leg's lifto� from the
ground and their energy damping capabilities at the time of collision, a much more
energy e�cient walk can be produced. To �nally construct such a system, it will
be necessary to further investigate model additions such as arms, movement in 3
dimensions, actuator dynamics and friction. Additionally, the challenging problem
of closed-loop control must be addressed, though with our solution of the open-loop
problem, this becomes already more tractable.
Appendix A
Identities
A.A De�nitions and Notation
We present here a list of de�nitions and notation used in this thesis.
�, _�, �� generalized positions, velocities, and accelerations of the
multibody system
Nb number of branches in multibody system
N , Nj, NL N refers to the number of links in a single-chain system,
Nj to the number of links in branch j, and NL to the total
number of links in the multibody system
dm;i, dm dm;i is the number of motion degrees of freedom for link i
and dm the total number of motion degrees of freedom
! 3-dimensional angular velocity vector
v 3-dimensional linear velocity vector
V = �H _� stacked spatial velocity vector consisting of NL 6-
dimensional spatial velocities Vi
�V stacked local spatial acceleration vector using local circle
derivatives
_V stacked spatial acceleration vector
160
161
_Vg stacked spatial accelerations due to the gravitational forces
~V � block diagonal spatial matrix containing in each diagonal
block the spatial cross product of the relative spatial velocity
between neighboring links
~ block diagonal spatial matrix containing in diagonal blocks
the spatial cross product of [!Ti0]T
M block diagonal spatial matrix containing in diagonal blocks
the link spatial inertia Mi
u vector of applied joint forces
M positive-de�nite mass-inertia matrix
C vector of Coriolis and centrifugal forces
G vector of gravitational forces
�i;i�1 transformation operator which maps spatial velocities from
joint i � 1 to joint i, the adjoint (transpose) maps spatial
forces from joint i to joint i� 1
�(j)i;(j)i�1 transformation operator between link i� 1 of branch j and
link i of branch j
E� stacked recursion operator with �i;i�1 on subdiagonal and
possible additional o�-diagonal terms for tree-structured
systems
E�(j), E�(j;k) E�(j) is the E� operator for jth branch in tree system, E�(j;k)
describes interaction between branch j and k
� lower triangular recursion matrix composed from E� and
used only in stacked notation
H block diagonal spatial matrix containing in diagonal blocks
the transformation matrix Hi which maps the joint veloci-
ties _� to the relative spatial velocity V �
i, its adjoint (trans-
pose) maps spatial forces f to applied joint forces u
162
P = ET PE +M articulated body inertia
D = HTPH forward dynamics operator
G = D�1HTP forward dynamics operator
K = GE� forward dynamics operator
�� = I �HG forward dynamics and projection operator
E = ��E�, recursion operators similar to E� and �
j = (j) branch index of predecessor branch of branch j
p
j= (j) branch index of pth predecessor branch of branch j
��f free joint accelerations which don't include e�ects of contact
forces
Nc number of contact points
dc;k, dc dc;k is the number of contact constraints at kth contact point
while dc is the total number of contact constraints
Vc, _Vc spatial velocities and accelerations at points of contact and
collision
Q block diagonal matrix with Nc blocks containing the con-
stant contact constraint mapping Qk taking spatial velocity
components to velocity contact or collision constraints
B (6 � Nc) � (6 � NL) spatial matrix containing in each row
of spatial matrices indexed by the number of contact con-
straints one transformation matrix taking the joint spatial
velocity to the contact point spatial velocity
Jc = QB�H constraint Jacobian
� = JcM�1JTc
interaction matrix used in contact and collision algorithms
fc = ���1Q _Vc stacked vector of spatial contact forces
fimp = ��1Q�Vc stacked vector of spatial impulsive forces from collision and
contact
163
Je = B�H Jacobian used for introducing external forces
fe spatial vector of external forces
�r branch index for branch upon which contact point r lies
�, _� reduced generalized velocity and acceleration vectors
KE, PE kinetic and potential energy
A.B Tilde Identities
The following tilde identities may easily be proved through direct multi-
plication. Let p and q be arbitrary 3-vectors.
~pp = 0 (A.1)
~pT = �~p (A.2)
~pq = �~qp (A.3)
g(~pq) = ~p~q � ~q~p (A.4)
~p~q = qpT � pT qI (A.5)
~p~q~qp = �~q~p~pq (A.6)
A.C Spatial Matrix Identities
We present here a list of identities that the spatial operators satisfy.
��1 = I � E� (A.7)
�1 = I � E (A.8)
�E� = E�� = �� I = �� (A.9)
E = E = � I = � (A.10)
�1� = I +HK� (A.11)
� �1 = I + �HK (A.12)
164
�H = H[I +K�H] (A.13)
K� = [I +K�H]K (A.14)
[I +K�H]�1
= [I �K H] (A.15)
��H = 0 (A.16)
E �H = ���H (A.17)
P �� = ��TP = ��TP �� (A.18)
HTP �� = 0 (A.19)
�� = �� �� (A.20)
��TP �� = ��TP = P �� (A.21)
M = P � ET�PE (A.22)
TM = P + � TP + P � (A.23)
D = HT TM H (A.24)
�~V �E� = E� ~V � ~V E� (A.25)
��~V �E�� = �~V � ~V � (A.26)
M = P � ET�PE� +KTDK (A.27)
TM� = TP + P � (A.28)
M�1HT�TM� = [I �K H]D�1HT TP +K (A.29)
Proof: Many of the following proofs may be found in a similar form in [44, 42].
[(A.7),(A.8)]: These identities follow from the calculation of the matrix product
�E�. The relationship with follows in the same manner.
[(A.9),(A.10)]: Rearranging the equation �(I�E�) = (I�E�)� = I gives the result.
The same holds for .
[(A.11),(A.12)]: Using the de�nitions listed in Section A.A and (A.7) and (A.8), it
follows that
I � �1 = E = E� � �E� = I � ��1 �HK
The result is then obtained by �rst multiplying the above equation by � on the left,
165
then on the right, and using (A.9) and (A.10).
[(A.13),(A.14)]: Using (A.11) and (A.12), we obtain the result:
�H = ( �1�)H = [I +HK�]H = H[I +K�H]
K� = K(� �1) = K[I + �HK] = [I +K�H]K
[(A.15)]: Using the matrix inversion lemma, we obtain a closed form expression for
the inverse of this term:
[I +K�H]�1 = I �K[I + �HK]�1�H
= I �K H
[(A.16),(A.17)]: Using the de�nitions found in Section A.A, it is easily shown that
GH = I. From this and the de�nition of �� , (A.16) follows. We now use this identity
to show (A.17):
E �H = �� ��H = ���H � ��H = ���H
[(A.18),(A.19)]: Both identities follow directly from the de�nitions of Section A.A
and multiplying out the terms. Note that (A.19) also implies that HTPE =
HTP � = 0.
[(A.20),(A.21)]:
�� �� = I � 2HG +HD�1HTPHD�1HTP = I �HG
��TP �� = P � 2PHD�1HTP + PHD�1HTPHD�1HTP
= P � PHD�1HTP
= P [I �HG] = [I �GTHT ]P
[(A.22),(A.23)]: This identity is a direct consequence of the de�nition of the P re-
cursion (2.72) and identity (A.24). Identity (A.23) follows from multiplying (A.22)
on the left by T and on the right by together with (A.10).
[(A.24)]: Multiplying (A.23) byHT and H from the left and right respectively gives
HT TM H = HTPH +HT � TPH +HTP � H :
166
Now use (A.19) to show that the last two terms are zero together with the de�nition
for D, D = HTPH.
[(A.25),(A.26)]: For a spatial vector X the following relationship holds:
g[�y;xX] = �y;xX��1y;x
Using this relationship with the spatial velocity recursion Vi = �i;i�1Vi�1 + Hi_�i
from (2.34) gives
~Vi�i;i�1 = �i;i�1 ~Vi�1 + ~V ��i;i�1
Equation (A.25) is the stacked version of this latter equation. Multiplying (A.25)
on the left and right by � and using (A.9) gives the next identity.
A.D Miscellaneous Identities
For any function g(�; _�), the following relationships hold:
@ _g
@��=
@g
@ _�(A.30)
@ _g
@ _�=
d
dt
@g
@ _�+@g
@�(A.31)
@ _g
@�=
d
dt
@g
@�(A.32)
Bibliography
[1] D. Agahi and K. Kreutz-Delgado, \A star topology dynamic model for e�-
cient simulation of multilimbed robotic systems," Proc. IEEE International
Conf. on Robotics and Automation, pp. 352{7, 1994.
[2] W.W. Armstrong, \Recursive Solution to the Equations of Motion of an N-link Manipulator," Fifth World Congress on Theory of Machines and Mech-
anisms, pp. 1343{46, 1979.
[3] U.M. Ascher, C. Hongsheng, L.R. Petzold and S. Reich, \Stabilization of con-
strained mechanical systems with DAEs and invariant manifolds,"Mechanics
of Structures and Machines, Vol. 23, No. 2, pp. 135{57, 1995.
[4] O.O. Badmus, S. Chowdbury, and C.N. Nett, \Nonlinear control of surge inaxial compression systems," Automatica, Vol. 32, No. 1, pp. 59{70, 1996.
[5] D. Bae and E. Haug, \A Recursive Formulation for Constrained MechanicalSystems Dynamics: Part I. Open-Loop Systems,"Mechanical Strtuctures and
Machines, Vol. 15, No. 3, pp. 359{382, 1987.
[6] J. Baillieul, S. Dahlgren, and B. Lehman, \Nonlinear control designs for sys-tems with bifurcations with applications to stabilization and control of com-pressors," Proc. 34th IEEE Conf. on Decision and Control, pp. 3062{7, 1995.
[7] J.A. Ball and J.W. Helton, \H1 control for Nonlinear Plants: Connectionswith Di�erential Games," 28th IEEE Conf. on Decision and Control, pp.
956{962, 1989.
[8] J.A. Ball, J.W. Helton, and M.L. Walker, \H1 control for nonlinear systemswith output feedback," IEEE Transactions on Automatic Control, Vol. 38,
No. 4, pp. 546{59, 1993.
[9] T. Ba�sar and G. Olsder, Dynamic Noncooperative Game Theory, 2nd ed.,
Academic Press, 1995.
[10] R.W. Beard and T.W. McLain, \Successive Galerkin Approximation Algo-rithms for Nonlinear Optimal and Robust Control," International Journal of
Control, Vol. 71, No. 5, pp. 717{43, 1998.
167
168
[11] R. Behnken, R. D'Andrea, and R.M. Murray, \Control of rotating stall in a
low-speed axial ow compressor using pulsed air injection: modeling, simula-
tions, and experimental validation," Proc. 34th IEEE Conf. on Decision and
Control, pp. 3056{61, 1995.
[12] D.P. Bertsekas and J.N. Tsitsiklis, Neuro-dynamic programming, Athena Sci-
enti�c, 1996.
[13] H. Brandl, R. Johanni, and M. Otter, \A Very E�cient Algorithm for the
Simulation of Robots and Similar Multibody Systems Without Inversion of
the Mass Matrix," IFAC/IFIP/IMACS Symposium, pp. 95{100, 1986.
[14] M.H. Breitner, \Real-time applicable feedback controller for di�erential
games," Proc. of the Sixth Interbaytional Symposium on Dynamic Games
and Applications, St-Jovite, Quebec, Canada, 1994.
[15] R.W. Brockett, A. Stokes, and F. Park, \A Geometrical Formulation of theDynamical Equations Describing Kinematic Chains," Proc. IEEE Interna-
tional Conference on Robotics and Automation, Vol. 2, pp. 637{41, 1993.
[16] C. Chevallereau, A. Formal'sky, and B. Perrin, \Low Energy Cost ReferenceTrajectories for a Biped Robot," Proc. IEEE International Conf. on Robotics
& Automation, pp. 1398{1404, 1998.
[17] C.K. Chow and D.H. Jacobson, \Studies of Human Locomotion via OptimalProgramming," Mathematical Biosciences, Vol. 10, pp. 239{306, 1971.
[18] P.M. Dower, Power Gain Analysis and Control of Nonlinear Systems, Ph.D.Thesis, Computer Engineering, University of Newcastle, Australia, 1998.
[19] J. Doyle, K. Glover, P.P. Khargonekar, and B. Francis, \State-space solu-tions to the standard H2 and H1 control problems," IEEE Transactions on
Automatic Control, Vol. 34, No. 8, pp. 831{47, 1989.
[20] J.C. Doyle, \Synthesis of robust controllers and �lters," Proc. 22nd IEEE
Conference on Decision and Control, pp. 109{14, 1983.
[21] K.M. Eveker, D.L. Gysling, C.N. Nett, and O.P Sharma, \Integrated Con-trol of Rotating Stall and Surge in Aeroengines," SPIE Conf. on Sensing,
Actuation, and Control in Aeropropulsion, 1995.
[22] M. Falcone and R. Ferreti, \Discrete time high-order schemes for viscositysolutions of Hamilton-Jacobi-Bellman equations," Numerische Mathematik,
Vol. 67, No. 3, pp. 315-344, 1994.
[23] R. Featherstone, Robot Dynamics Algorithms, Kluwer Academic Publishers,
1987.
169
[24] K.S. Fu, R.C. Gonzalez, and C.S.G. Lee, Robotics: control, sensing, vision,
and intelligence, McGraw-Hill, 1987.
[25] Y. Fujimoto and A. Kawamura, \Robust biped walking with active interac-
tion control between foot and ground," Proc. IEEE International Conf. on
Robotics and Automation, pp. 2030{5, 1998.
[26] P.E. Gill, W. Murray, M.A. Saunders, M.H.Wright, User's Guide for NPSOL,
Version 5.0.
[27] A. Goswami, B. Espiau, and A. Keramane, \Limit Cycles in a Passive
Compass Gait Biped and Passivity-Mimicking Control Laws," Autonomous
Robots, Vol. 4, pp. 273{286, 1997.
[28] M. Green and D.J.N Limebeer, Linear Robust Control, Prentice-Hall, Engle-wood Cli�s, NJ, 1995.
[29] M. Hardt, J.W. Helton, and K. Kreutz-Delgado, \Numerical Solution of Non-linearH2 and H1 Control Problems with Application to Jet Engine Control,"Proc. 36th IEEE Conf. on Decision and Control, Vol. 3, pp. 2317{22, 1997.
[30] M. Hardt and K. Kreutz-Delgado, \Using the Algebraic Structure of Roboticsin Control Design," Proc. IEEE 36th Conf. on Decision and Control, pp.4850{55, 1997.
[31] M. Hardt, K. Kreutz-Delgado, and J. William Helton, \Minimal Energy Con-
trol of a Biped Robot with Numerical Methods and a Recursive SymbolicDynamic Model," Proc. 37th IEEE Conference on Decision and Control, pp.413{6, 1998.
[32] J.W. Helton, \An H1 approach to control," Proc. 22nd IEEE Conference on
Decision and Control, pp. 607{12, 1983.
[33] P. Hiltmann, K. Chudej, and M. Breitner, \Eine modi�zierte Mehrzielmeth-
ode zur L�osung von Mehrpunkt-Randwertproblemen: Benutzeranleitung,"Report Nr. 14, Mathematics Department, TU-Munich, 1993.
[34] J.M. Hollerbach, \A Recursive Lagrangian Formulation of Manipulator Dy-
namics and A Comparative Study of Dynamics Formulation Complexity,"
IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-10, No. 11,
pp. 730{6, 1980.
[35] W. Hooker and G. Margulies, \The Dynamical Attitude Equations for an n-
Body Satellite," Journal of Astronautical Sciences, Vol. 12, pp. 123{8, 1965.
[36] R. Huston and C. Passerello, \On Multi-Rigid-Body System Dynamics,"Computers and Structures, Vol. 10, pp. 439{46, 1979.
170
[37] A. Isidori and A. Astol�, \Disturbance attenuation and H1 control via mea-
surement feedback in nonlinear systems," IEEE Trans. on Automatic Control
Vol. 37, No. 9, pp. 1283{1293, 1992.
[38] A. Jain, \Uni�ed Formulation of Dynamics for Serial Rigid Multibody Sys-
tems," Journal of Guidance, Control, and Dynamics, Vol. 14, pp. 531{542,
1991.
[39] A. Jain, \Uni�ed Formulation of Dynamics for Serial Rigid Multibody Sys-
tems," Jet Propulsion Laboratory, Unpublished lecture notes, 1989.
[40] A. Jain and G. Rodriguez, \Recursive exible multibody system dynamics
using spatial operators," Journal of Guidance, Control, and Dynamics, Vol.
15, No. 6, pp. 1453{66, 1992.
[41] A. Jain and G. Rodriguez, \An Analysis of the Kinematics and Dynamicsof Underactuated Manipulators," IEEE Trans. on Robotics and Automation,Vol. 9, No. 4, pp. 411{22, 1993.
[42] A. Jain and G. Rodriguez, \Linearization of Manipulator Dynamics UsingSpatial Operators," IEEE Transactions on Systems, Man, and Cybernetics,
Vol. 33, No. 1, pp. 239{48, 1993.
[43] A. Jain and G. Rodriguez, \Base-Invariant Symmetric Dynamics of Free-Flying Manipulators," IEEE Trans. on Robotics and Automation, Vol. 11,No. 4, pp. 585{97, 1995.
[44] A. Jain and G. Rodriguez, \Diagonalized Lagrangian Robot Dynamics,"IEEE Transactions on Robotics and Automation, Vol. 11, No. 4, pp. 571{584, 1995.
[45] A. Jain, G. Rodriguez, and K. Kreutz-Delgado, \Multi-arm grasp and manip-ulation of objects with internal degrees of freedom," Proc. 29th IEEE Conf.
on Decision and Control, pp. 3110{11, 1990.
[46] S. Kajita and K. Tani, \Experimental Study of Biped Dynamic Walking,"
IEEE Control Systems, pp. 13{19, 1996.
[47] T.R. Kane and D.A. Levinson, \Formulation of Equations of Motion for Com-
plex Spacecrafts," Journal of Guidance and Control, Vol. 3, No. 2, pp. 99{112,1980.
[48] O. Khatib, \A Uni�ed Approach for Motion and Force Control of RobotManipulators: The Operational Space Formulation," IEEE Transactions on
Robotics and Automation, Vol. RA-3, No. 1, pp. 43{53, 1987.
[49] N. Kir�canski, M. Vukobratovi�c, A. Timcenko, M. Kir�canski, \Symbolic Mod-
eling in Robotics: Genesis, Application, and Future Prospects," Journal of
Intelligent and Robotic Systems, Vol. 8, pp. 1{19, 1993.
171
[50] H. Kreim, B. Kugelmann, H.J. Pesch, and M.H. Breitner, \Minimizing the
maximum heating of a re-entering space shuttle: an optimal control problem
with multiple control constraints," Optimal Control Applications & Methods,
Vol. 17, No. 1, pp. 45{69, 1996.
[51] K. Kreutz-Delgado, A. Jain, and G. Rodriguez, \Recursive Formulation of
Operational Space Control," The International Journal of Robotics Research,
Vol. 11, No. 4, pp. 320{28, 1992.
[52] M. Krsti�c, D. Fontaine, P.V. Kokotovi�c, and J.D. Paduano, \Useful Nonlin-
earities and Global Stabilization of Bifurcations in a Model of Jet Engine
Surge and Stall," IEEE Trans. on Automatic Control, Vol. 43, No. 12, pp.
1739{45, 1998.
[53] M. Krsti�c, J.M. Protz, J.D. Paduano, and P.V. Kokotovi�c, \BacksteppingDesigns for Jet Engine Stall and Surge Control," Proc. 34th IEEE Conf. on
Decision and Control, pp. 3049{55, 1995.
[54] H.J. Kushner and P.G. Dupuis, Numerical methods for stochastic control prob-lems in continuous time, Springer, New York, 1992.
[55] H.G. Kwatny and G.L. Blankenship, \Symbolic Construction of Models forMultibody Dynamics," IEEE Trans. on Robotics and Automation, Vol. 11,No. 2, pp. 271{281, 1995.
[56] T. Lauvdal, R.M.Murray, and T.I. Fossen, \Stabilization of Integrator Chainsin the Presence of Magnitude and Rate Saturations: a Gain Scheduling Ap-
proach," Proc. 36th IEEE Conf. on Decision and Control, pp. 4004{5, 1997.
[57] F. Lewis, C. Abdallah, and D. Dawson, Control of Robot Manipulators,Macmillan, 1993.
[58] D.C. Liaw and E.H. Abed, \Active Control of Compressor Stall Inception:A Bifurcation-theoretic Approach," Automatica, Vol. 32, No. 1, pp. 109{115,
1996. (also in Proc. of the IFAC Nonlinear Control Systems Design Sympo-
sium, Bordeaux, France, 1992)
[59] K.W. Lilly and D.E. Orin, \E�cient Dynamic Simulation of Multiple Chain
Robotic Mechanism," Journal of Dynamic Systems, Measurement, and Con-
trol, Vol. 116, pp. 223{31, 1994.
[60] J. Luh, M. Walker, and R. Paul, \On-line computational scheme for mechan-
ical manipulators," ASME Journal of Dynamic Systems, Measurement, and
Control, Vol. 102, No. 2, pp. 69{76, 1980.
[61] C.A. Mansoux, J.D. Setiawan, D.L. Gysling, and J.D. Paduano, \Distributed
nonlinear modeling and stability analysis of axial compressor stall and surge,"
1994 American Control Conf., Vol. 2, pp. 2305{16, 1994.
172
[62] Matlab 4.2c, The MathWorks, Inc., 1995.
[63] W.M. McEneaney and K.D. Mease, \Nonlinear H1Control of Aerospace
Plane Ascent", Proc. 34th IEEE Conf. on Decision and Control, pp. 3994{5,
1995.
[64] T. McGeer, \Passive Dynamic Walking," The International Journal of
Robotics Research, Vol. 9, No. 2, pp. 62{82, 1990.
[65] F.K. Moore and E.M. Greitzer, \A Theory of Post-Stall Transients in Ax-
ial Compression Systems: Part I { Development of Equations," Journal of
Engineering for Gas Turbines and Power, Vol. 108, pp. 68{76, 1986.
[66] R.M. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to Robotics,
CRC Press, 1994.
[67] S.Y. Oh and D.E. Orin, \Dynamic Computer Simulation of Multiple Closed-
Chain Robotic Mechanisms," Proc. IEEE International Conf. Robotics and
Automation, pp. 15{20, 1986.
[68] D.E. Orin and S.Y. Oh, \Control of Force Distribution in Robotic Mecha-
nisms Containing Closed Kinematic Chains," Journal of Dynamic Systems,
Measurement, and Control, Vol. 102, pp. 134{41, 1981.
[69] J.D. Paduano, L. Valavani, A.H. Epstein, E.M. Greitzer, G.R. Guenette,\Modeling for control of rotating stall," Automatica, Vol. 30, No. 9, pp. 1357{
73, 1994.
[70] F.C. Park, J.E. Bobrow, and S.R. Ploen, \A Lie Group Formulation of RobotDynamics," The International Journal of Robotics Research, Vol. 14, No. 6,
pp. 609{618, 1995.
[71] H.J. Pesch, I. Gabler, S. Miesbach, M.H. Breitner, \Synthesis of optimalstrategies for di�erential games by neural networks," Annals of the Interna-
tional Society of Dynamic Games, Vol. 3, pp. 78{106, 1996.
[72] S. Ploen, Geometric Algorithms for the Dynamics and Control of Multibody
Systems, Ph.D. Thesis, University of California, Irvine, 1997.
[73] D. Pogorelov, \Di�erential-algebraic equations in multibody system model-ing," Numerical Algorithms, Vol. 19, No. 4, pp. 183{94, 1998.
[74] R. Roberson and R. Schwertassek, Dynamics of Multibody Systems, Springer-
Verlag, 1988.
[75] R. Roberson and J. Wittenburg, \A Dynamical Formalism for an Arbitrary
Number of Interconnected Rigid Bodies," Proc. Third Congress IFAC, Vol.
1, Book 3, Paper No. 46D, 1997.
173
[76] G. Rodriguez, \Kalman Filtering, Smoothing, and Recursive Robot Arm For-
ward and Inverse Dynamics," IEEE Journal of Robotics and Automation, Vol.
3, No. 6, pp. 624{639, 1987.
[77] G. Rodriguez, and A. Jain, and K. Kreutz-Delgado, \Spatial Operator Alge-
bra for Multibody System Dynamics," Journal of the Astronautical Sciences,
Vol. 40, No. 1, pp. 27{50, 1992.
[78] G. Rodriguez, K. Kreutz-Delgado, and A. Jain, \A Spatial Operator Algebra
for Manipulator Modeling and Control," International Journal of Robotics
Research, Vol. 10, No. 4, pp. 371{81, 1991.
[79] J. Rose and J.G. Grimble, Human Walking, Williams & Wilkins, 1994.
[80] M. Rostami and G. Bessonnet, \Impactless sagittal gait of a biped robotduring the single support phase," Proc. IEEE International Conf. on Robotics
& Automation, pp. 1385{91, 1998.
[81] L. Roussel, C. Canudas de Wit, and A. Goswami, \Generation of EnergyOptimal Complete Gait Cycles for Biped Robots," Proc. IEEE International
Conf. on Robotics & Automation, pp. 2036{41, 1998.
[82] P. Saint-Pierre, \Numerical approximation of the value function for controlproblems with state constraints: a viability approach," Proc. of IFAC Sym-
posium on Nonlinear Control Systems Design, Vol. 2, pp. 529{34, 1995.
[83] W. Schiehlen, \Multibody System Dynamics: Roots and Perspectives,"Multibody System Dynamics, Vol. 1, No. 2, pp. 149{188, 1997.
[84] J.M. Selig, Geometrical Methods in Robotics, Springer, 1996.
[85] C.L. Shih and W. Gruver, \Control of a Biped Robot in the Double-SupportPhase," IEEE Transactons on Systems, Man, and Cybernetics, Vol. 22, No.4, pp. 729{35, 1992.
[86] B. Simeon, \MBSPACK{numerical integration software for constrained me-chanical motion," Surveys on Mathematics for Industry, Vol. 5, No. 3, pp.
169{202, 1995.
[87] M.W. Spong and M. Vidyasagar, Robot Dynamics and Control, John Wiley& Sons, 1989.
[88] V. Stejskal and M. Val�asek, Kinematics and Dynamics of Machinery, MarcelDekker, 1996.
[89] S. Swinnen, H. Heuer, J. Massion, and P. Casaer, Interlimb Coordination.
Neural, Dynamical, and Cognitive Constraints, Academic Press, 1994.
174
[90] A.R. Teel, O.E. Kaiser, and R.M. Murray, \Uniting Local and Global Con-
trollers for the Caltech Ducted Fan," Proc. IEEE 36th Conf. on Decision and
Control, pp. 1539{43, 1997.
[91] A.J. van der Schaft, \L2{gain analysis of nonlinear systems and nonlinear
state feedback H1 control," IEEE Transactions on Automatic Control, Vol.
37, No. 6, pp. 770{84, 1992.
[92] A.J. van der Schaft, \Nonlinear state space H1 control theory" in: H.L.
Trentelman and J.C. Willems, Essays on Control: Perspectives in the Theory
and Applications, Birkh�auser, Berlin, 1993.
[93] A.F. Vereshchagin, \Computer Simulation of the Dynamics of Complicated
Mechanisms of Robot Manipulators," Engineering Cybernetics, Vol. 6, pp.
65{70, 1974.
[94] O. von Stryk, Numerische L�osung optimaler Steuerungsprobleme: Diskreti-
sierung, Parameteroptimierung und Berechnung der Adjungierten Variablen,
Ph.D. Thesis, Department of Mathematics, TU-Munich, 1995.
[95] O. von Stryk, User's Guide for DIRCOL. A direct collocation method for the
numerical solution of optimal control problems, 1995.
[96] M. Vukobratovic and A. Tuneski, \Adaptive control of single rigid roboticmanipulators interacting with dynamic environment - an overview," Journalof Intelligent and Robotic Systems: Theory and Applications, Vol. 17, No. 1,pp. 1{30, 1996.
[97] M. Walker and D. Orin, \E�cient Dynamic Computer Simulation of RoboticMechanisms," ASME Journal of Dynamic Systems, Measurement, and Con-
trol, Vol. 104, No. 3, pp. 205{211, 1982.
[98] Y. Wang and R.M. Murray, \E�ects of Noise, Magnitude Saturation, and
Rate Limits on Rotating Stall Control," Proc. 36th IEEE Conf. on Decision
and Control, pp. 4682{87, 1997.
[99] J.M. Wendlandt and S.S. Sastry, \Recursive Workspace Control of MultibodySystems: A Planar Biped Example," Proc. 35th IEEE Conf. on Decision and
Control, pp. 3575{80, 1996.
[100] J. Wittenburg, Dynamics of Systems of Rigid Bodies, B.G. Teubner,Stuttgart, 1977.
[101] G. Zames, \Optimal sensitivity and feedback: weighted seminorms, approxi-mate inverses, and plant invariant schemes," Proceedings Allerton Conference,
1979.
175
[102] K. Zhou, J. Doyle, and K. Glover, Robust and Optimal Control, Prentice-Hall,
Englewood Cli�s, NJ, 1996.
