Advanced analytical dynamics 9781107179608, 9781316832301, 1107179602

Citation preview

Advanced Analytical Dynamics This book provides a unique bridge between the foundations of analytical mechanics and application to multibody dynamical systems. It is intended as a textbook, particularly well suited for graduate students seeking an understanding of the theoretical underpinnings of analytical mechanics, as well as modern task space approaches for representing the resulting dynamics that can be exploited for real-world problems in areas such as biomechanics and robotics. Established principles in mechanics are presented in a thorough and modern way. The chapters build up from general mathematical foundations to an extensive treatment of kinematics and then to a rigorous treatment of conservation and variational principles in mechanics. Parallels are drawn between the different approaches, providing the reader with insights that unify his or her understanding of analytical dynamics. Additionally, a unique treatment is presented on task space dynamical formulations that map traditional configuration space representations into more intuitive geometric spaces. Vincent De Sapio is a research scientist at HRL Laboratories in Malibu, California. He has more than 40 publications and 16 patents (issued or pending) in the areas of multibody dynamics, robotics, biomechanics, control theory, and human motion synthesis and simulation. Dr. De Sapio is a senior member of IEEE and a member of ASME and Sigma Xi. He is a founding member of the IEEE Technical Committee on Human Movement Understanding. He received his Ph.D. and M.S. from Stanford University and his B.S. from Rensselaer Polytechnic Institute, all in mechanical engineering.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:43:46, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

The light shines in the darkness, and the darkness has not overcome it.

John 1:5

S. D. G.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:43:46, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Advanced Analytical Dynamics Theory and Applications VINCENT DE SAPIO HRL Laboratories LLC

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:43:46, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

University Printing House, Cambridge CB2 8BS, United Kingdom One Liberty Plaza, 20th Floor, New York, NY 10006, USA 477 Williamstown Road, Port Melbourne, VIC 3207, Australia 4843/24, 2nd Floor, Ansari Road, Daryaganj, Delhi - 110002, India 79 Anson Road, #06-04/06, Singapore 079906 Cambridge University Press is part of the University of Cambridge. It furthers the University’s mission by disseminating knowledge in the pursuit of education, learning, and research at the highest international levels of excellence. www.cambridge.org Information on this title: www.cambridge.org/9781107179608 DOI: 10.1017/9781316832301 © Vincent De Sapio 2017 This publication is in copyright. Subject to statutory exception and to the provisions of relevant collective licensing agreements, no reproduction of any part may take place without the written permission of Cambridge University Press. First published 2017 Printed in the United States of America by Sheridan Books, Inc. A catalogue record for this publication is available from the British Library. Library of Congress Cataloging-in-Publication Data Names: De Sapio, Vincent, 1968– author. Title: Advanced analytical dynamics : theory and applications / Vincent De Sapio (HRL Laboratories LLC). Description: Cambridge, United Kingdom ; New York, NY : Cambridge University Press, 2017. | Includes bibliographical references and index. Identifiers: LCCN 2016036860| ISBN 9781107179608 (hardback ; alk. paper) | ISBN 1107179602 (hardback ; alk. paper) Subjects: LCSH: Dynamics. | Mechanics, Analytic. Classification: LCC QA845 .D42 2017 | DDC 620.1/04–dc23 LC record available at https://lccn.loc.gov/2016036860 ISBN 978-1-107-17960-8 Hardback Cambridge University Press has no responsibility for the persistence or accuracy of URLs for external or third-party Internet Web sites referred to in this publication and does not guarantee that any content on such Web sites is, or will remain, accurate or appropriate.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:43:46, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Contents

List of Illustrations List of Tables Preface Notation 1

2

3

4

5

page vii xi xiii xvii

Introduction

1

1.1 Historical Background 1.2 Devices That Illustrate Principles of Analytical Dynamics 1.3 Scope of This Book

1 5 8

Mathematical Preliminaries

10

2.1 2.2 2.3 2.4

10 23 28 31

Linear Systems Differential Geometry Optimization Exercises

Kinematics of Discrete Systems

36

3.1 3.2 3.3 3.4 3.5

36 54 69 76 77

Spherical Kinematics Spatial Kinematics Kinematic Chains Kinematic Constraints and Degrees of Freedom Exercises

Conservation Principles

83

4.1 The Newton-Euler Principle 4.2 Exercises

83 99

Zeroth-Order Variational Principles

101

5.1 Virtual Displacements 5.2 D’Alembert’s Principle of Virtual Work 5.3 Hamilton’s Principle of Least Action

101 101 128

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:48:10, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

vi

6

7

8

9

10

Contents

5.4 Canonical Hamiltonian Formulation 5.5 Elimination of Multipliers 5.6 Exercises

142 145 147

First-Order Variational Principles

151

6.1 6.2 6.3 6.4

151 151 179 185

Virtual Velocities Jourdain’s Principle of Virtual Power Kane’s Formulation Exercises

Second-Order Variational Principles

188

7.1 7.2 7.3 7.4 7.5

188 188 201 206 211

Virtual Accelerations Gauss’s Principle Gauss’s Principle of Least Constraint Gibbs-Appell Formulation Exercises

Dynamics in Task Space

214

8.1 Task Space Framework 8.2 Constrained Dynamics in Task Space 8.3 Exercises

214 227 233

Applications to Biomechanical Systems

235

9.1 Musculoskeletal and Neuromuscular Dynamics 9.2 Constrained Dynamics of Biomechanical Systems

235 245

Software for Analytical Dynamics

263

10.1 General Purpose Mathematical Software 10.2 Dedicated Multibody Dynamics Software

263 268

Appendix Inclusion of Flexible Bodies

269

A.1 A.2 A.3

269 270 273

Continuum Kinematics Continuum Dynamics Subsystem Assembly

References Index

275 279

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:48:10, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Illustrations

1.1 1.2 1.3 1.4 1.5 2.1 2.2 2.3 2.4 2.5 2.6 2.7 2.8 2.9 2.10 3.1 3.2 3.3 3.4 3.5 3.6 3.7 3.8 3.9 3.10 3.11 3.12 3.13 3.14 3.15 3.16 3.17 3.18 3.19

Plate no. 12 of Bizzarie di Varie Figure Historical progression of key principles of analytical dynamics Some devices composed of branching kinematic chains Some devices that operate under holonomic constraints Some devices that operate under nonholonomic constraints Triangle ABC demonstrating the law of cosines Vector coordinates with respect to a basis, B Least norm solution and space of all solutions Relationship between extrinsic, geodesic, and normal curvature A geodesic showing zero geodesic curvature Minimization of J pxq “ xT Bx subject to the constraint, Ax “ y Two parametric space curves ` ˘2 Implicit surface f “ 4 ` x2 `` y2 ´ z ´ 13 cosp3xq cosp3yq “ 0 ˘ Parametric surface rpu, vq “ u, v, ´ 25 pu2 ` v 2 q ` cosp2uq cosp2vq Implicit surface f “ z ´ xy “ 0 Rotation of frame B relative to frame A Axis-angle representation describing an arbitrary rotation Euler angle sequence (xyz) Differential spin, θ , about a fixed instantaneous axis Instantaneous spin of a body about an axis First two quaternion components derived from gyroscope data Last two quaternion components derived from gyroscope data Euler angle (β) time history derived from gyroscope data Euler angle (α and γ ) time history derived from gyroscope data A homogenous transform A screw description Transformation sequence of a screw displacement Body with acceleration components known at three different locations Center of mass position and orientation time history Center of mass acceleration x component Center of mass acceleration y and z components Center of mass velocity and position x components Center of mass velocity and position y component Center of mass velocity and position z component

page 2 4 6 7 8 11 14 19 27 28 31 32 33 34 34 37 38 40 45 49 50 51 52 53 55 56 57 61 63 63 64 65 66 67

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:57:11, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

viii

Illustrations

3.20 Reentry body trajectory 3.21 Coning angle of the reentry body 3.22 Denavit-Hartenberg parameters 3.23 Branching and closed chain systems 3.24 Rotation of frame B relative to frame A 3.25 Frame representations of a RRR joint mechanism 3.26 Frame representations of a PRR joint mechanism 3.27 A 2 degree-of-freedom serial chain robot (kinematics) 4.1 Free-body diagram showing external forces and moments 4.2 Angular momentum of a body about an arbitrary origin point 4.3 A cone with a base radius of R and height of h in the z direction 4.4 A 3 degree-of-freedom serial chain robot 4.5 Animation frames and plots from the simulation of a serial chain robot 4.6 A cylindrical tube and a cuboid 4.7 A 2 degree-of-freedom serial chain robot (dynamics) 5.1 A virtual displacement of a rigid body 5.2 A 4 degree-of-freedom serial chain robot 5.3 Animation frames and plots from the simulation of a serial chain robot 5.4 Constrained structures involving closed loops and algebraic dependencies 5.5 Configuration space constrained-motion manifold, Q p 5.6 Stewart platform actuated by six prismatic struts 5.7 Constraint forces associated with loop closures 5.8 Parallel mechanism consisting of serial chains with loop closures 5.9 Animation frames and plots from the simulation of a parallel mechanism 5.10 Plots of Lagrange multipliers for a parallel mechanism 5.11 A gyroscope supported by a two-axis gimballed frame 5.12 Animation frames and plots from the simulation of a gimballed gyroscope 5.13 Kinetic and potential energy and conservation of total energy 5.14 A 2 degree-of-freedom serial chain robot (d’Alembert) 5.15 A two-link planar slider-crank mechanism 5.16 A planar four-bar linkage 5.17 A three-link planar slider-crank mechanism 6.1 A virtual velocity of a rigid body 6.2 A rolling disk with nonholonomic constraints 6.3 Animation frames and plots from the simulation of a nonholonomic rolling disk 6.4 Constraint forces exerted on the disk at the contact point 6.5 A rolling ball with nonholonomic constraints 6.6 Animation frames and plots from the simulation of a rolling ball 6.7 Plots of the orientation of a rolling ball and moment wheel angles 6.8 Two wheels connected by an axle with nonholonomic constraints

68 68 69 77 78 80 81 82 85 87 91 96 98 99 100 103 112 113 115 116 119 120 122 126 127 135 139 140 147 148 149 149 153 164 167 168 168 170 172 173

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:57:11, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Illustrations

6.9 6.10 6.11 6.12 6.13 6.14 7.1 7.2 7.3 7.4 7.5 7.6 7.7 7.8 8.1 8.2 8.3 8.4 8.5 8.6 8.7 8.8 8.9 8.10 8.11 8.12 9.1 9.2 9.3 9.4 9.5 9.6 9.7 9.8 9.9 9.10 9.11 9.12 9.13

Animation frames and plots from the simulation of the axle and rolling wheels Plots of the wheel angles for the rolling wheel assembly A rolling disk with nonholonomic constraints (Kane’s method) Animation frames and plots from the simulation of a rolling disk (Kane’s method) A simplified version of the rolling disk A two-wheeled apparatus A virtual acceleration of a rigid body The Gauss function, G, minimized subject to the constraints Geodesic force-free paths on a constrained-motion manifold Solving for geodesic force-free paths on a constrained-motion manifold A rolling disk with nonholonomic constraints (Gibbs-Appell method) A planar four-bar linkage (Gauss) ` ˘2 Implicit surface φ “ 4 ` x2 ` y2 ´ z ´ 13 cosp3xq cosp3yq “ 0 A simplified version of the rolling disk (Gibbs-Appell method) Task descriptions for serial and branching chains A 4 degree-of-freedom serial chain robot (task space dynamics) Animation frames and plots from the simulation of a serial chain robot Plots of the robot task space gravity vector A 3 degree-of-freedom serial chain robot (task space dynamics) Kinetic energy ellipsoids for a sequence of configurations Belted ellipsoids for a sequence of configurations Task description for a multibody system with loop constraints Parallel mechanism (task space dynamics) Animation frames and plots from the simulation of a parallel mechanism A 2 degree-of-freedom serial chain robot (task space dynamics) A two-link planar slider-crank mechanism (task space dynamics) Active state musculotendon model Neuromuscular and musculoskeletal system (feed-forward path) Simplified model of the human arm consisting of three generalized coordinates Simplified model of the human arm actuated by 24 muscles Muscle force-length-rate surface at full activation Animation frames and plots from the arm simulation Plots of neural excitations and muscle activations Various constituents of the shoulder complex Parameterization of a shoulder model Muscle paths spanning the shoulder complex Muscle moment arms for the deltoid muscles Muscle forces and moment capacities for the deltoid muscles Parallel and serial parts of a humanoid shoulder complex

ix

177 178 182 184 185 186 190 202 204 206 210 212 212 213 215 217 218 219 221 222 223 227 231 232 233 234 237 238 240 242 243 244 245 247 248 252 254 255 256

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:57:11, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

x

Illustrations

9.14 9.15 9.16 9.17 10.1 10.2 10.3 10.4 A.1

Humanoid shoulder complex actuated by four prismatic struts Constraint forces associated with loop closures in the humanoid shoulder complex Humanoid shoulder complex showing task point Animation frames and plots from the simulation of a humanoid shoulder complex Mathematica by Wolfram Research Inc. Simscape Multibody by The MathWorks Inc. SystemModeler by Wolfram Research Inc. OpenSim biomechanical simulation software incorporating Simbody Eight-node hexahedral element

256 258 260 261 264 265 266 267 271

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:57:11, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Tables

3.1 3.2 3.3 9.1 9.2 9.3

Dualities between the rotation matrix and the unit quaternion Denavit-Hartenberg parameters for RRR mechanism Denavit-Hartenberg parameters for PRR mechanism Kinematics for a 3 degree-of-freedom human arm model Muscle properties for the 24 muscles used in the model of Figure 9.4 Shoulder kinematics using a nonminimal set of coordinates

47 79 80 240 242 249

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 18:02:04, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 18:02:04, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Preface

This book addresses the analytical dynamics of multibody systems and is intended for a one- to two-semester advanced graduate-level course in analytical dynamics. The emphasis is on a solid theoretical foundation with examples that concretely illustrate the theory. I have included a chapter on the fundamental mathematics that is helpful in navigating the principles of dynamics. This includes coverage of linear systems and differential geometry. A chapter on kinematics, the study of the geometry of motion, follows. The first chapter, on dynamics, addresses conservation principles, fundamentally the conservation of momentum embodied in the Newton-Euler Principles. Historically, analytical mechanics (dynamics) has referred to the so-called variational principles, rooted in the calculus of variations. Three chapters cover zeroth-, first-, and second-order variational principles, respectively. Lagrangian and Hamiltonian mechanics are among the more well-known formulations arising from variational principles covered in this book. I also cover important, but lesser known, higher-order principles, including Jourdain’s Principle of Virtual Power, Gauss’s Principle of Least Constraint, and Hertz’s Principle of Least Curvature, as well as Kane’s formulation and the Gibbs-Appell formulation. As an aside, it is worth noting that modern theoretical physics emerged out of the classical variational principles. Einstein’s general theory of relativity is commonly formulated using Lagrangian mechanics. Dirac was the first to use the Lagrangian in quantum mechanics and provided separate formulations of quantum mechanics and general relativity based on the Hamiltonian formalism. He also provided a generalized formulation of constrained Hamiltonian systems. Additionally, Feynman’s path integral formulation of quantum mechanics has its classical ancestry in Hamilton’s Principle of Least Action. After the chapters on the variational principles, I have included a chapter on an alternate formulation of classical dynamics that has found significant utility in the control of robotic systems. The so-called task space formulation of dynamics was pioneered by Khatib under the name of operational space dynamics. It provides a transformation of the configuration space description of system dynamics into a more convenient task-oriented description. An applications-oriented chapter is included on biomechanical systems. This provides a basic overview of musculoskeletal and neuromuscular biomechanics with extensive coverage of application examples using actual anthropometric and muscle property data. The final chapter provides a brief survey of some analytical dynamics software. This is not intended to provide exhaustive coverage but only some examples of general purpose mathematical software with extensions for multibody dynamics, as well as dedicated multibody dynamics software, both commercial

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:36:48, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.001

xiv

Preface

and open source. An appendix is included that touches upon the application of continuum mechanics to flexible multibody systems, which include both rigid and deformable bodies. It is my intention to extend this appendix into a fully integrated book chapter in a future edition. I have tried to provide at least one example to illustrate each important concept covered in the book. The examples presented are thoroughly worked out in the text, so the reader should have no trouble following the methodology. In many cases, numerical simulation results are also provided. Some simple integration schemes are covered, and the reader is encouraged to explore the examples on his or her own and use any preferred mathematical software (e.g., Mathematica, Matlab) to generate simulation results. The reader would also benefit from employing mathematical software in addressing some of the exercises at the end of selected chapters. These exercises are often multipart and can involve fairly extensive mathematical computations. I would like to thank a number of my past teachers who helped impart to me not only an understanding and insight related to the topics covered in this book but also a sense of the vast expanse of applications for analytical dynamics. These include Oussama Khatib, Scott Delp, Jean Heegaard, Bernie Roth, Ken Waldron, and others. A number of other teachers have contributed to my understanding of areas related to analytical dynamics. These include Stephen Boyd, Steve Rock, Sanjay Lall, Ron Fedkiw, Peter Pinsky, Charles Steele, Edward Goldstein, and Terry Sanger. My academic colleagues have provided productive interaction as well. These include Jaeheung Park, Luis Sentis, François Conti, Mike Zinn, James Warren, Emel Demircan, Jinsung Kwon, Dongjun Shin, Anya Petrovskaya, Peter Thaulad, Oliver Brock, Vincent Padois, Kate Saul, Rob Siston, Jeff Reinbolt, and others. Although not directly related to the content of this book, many colleagues at HRL (formerly Hughes Research Laboratories) whom I have worked with on various projects have provided professional support and fruitful interaction. Among these individuals are Rajan Bhattacharyya, Jaehoon Choe, Yang Chen, Jose Cruz-Albrecht, Mike Daily, Son Dao, Chong Ding, Darren Earl, Karim El DeFrawy, Stephanie Goldfarb, Heiko Hoffmann, Mike Howard, Qin Jiang, Deepak Khosla, Ken Kim, Tiffany Kim, Dmitiry Korchev, Joonho Lee, Tsai-ching Lu, Charles Martin, Kevin Martin, Connie Ni, Aleksey Nogin, Yuri Owechko, Dave Payton, Matt Phillips, Praveen Pilly, Amir Rahimi, Shankar Rao, Shane Roach, Stephan Salas, Narayan Srinivasan, Nigel Stepp, Ryan Uhlenbrock, and others. Special thanks go to HRL’s patent counsel, George Rapacki, my administrative assistant, Jennifer Greene, and Joonho Lee, who performed a technical proofread of part of the manuscript. In addition to working with a talented group of people, HRL’s location provides me with inspiring views of the Pacific Ocean from a bluff overlooking the Malibu coastline in Southern California. It has been a great pleasure working with Cambridge University Press on this book. I would like to thank Steve Elliot, Mark Fox, and Rebecca Rom-Frank at Cambridge. I would also like to thank Holly Monteith for copy editing the manuscript, and Vijay Bhatia for managing the book composition and typesetting. The external technical reviewers provided valuable constructive assessments of the early draft, for which I am

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:36:48, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.001

Preface

xv

grateful. A special extra note of thanks goes to Steve Elliot who guided me through the manuscript evaluation, approval, and contract-related processes. A number of people in my life made this book possible, not by technical contribution, but by the consistent impact they have had on my life. First and foremost, my parents, Martin and Lucia, shaped my life in ways that were profound. As immigrants from Italy, both of them embodied the American dream and built their lives upon hard work, commitment to family, and an enduring faith. My siblings, Gaetano, Antonio, Maria, Carmine, and particularly my sisters, Rita and Sally, nurtured me throughout my life and continue to impact my life in positive ways. I can not sufficiently express my gratitude to them for the counsel they provide me in life. I would also like to thank Tom Stephen, who has provided me with his friendship and steadfast spiritual guidance, and my inlaws, Bob and Joyce Prindle, who have always encouraged me. The greatest joys in my life are my wife, Robin, and my two boys, Robbie and Marty. Robin has stuck by me when I thought no one would. She is a source of enthusiasm, compassion, and humor, and she is my rock. Robbie and Marty inspire me in ways that only a toddler and a six-year-old can. In them I see innocence (and a little mischief), a complete passion for life, and warm hearts. They are my dearest treasures in life. Finally, I thank God for all the blessings that He has provided in my life. It is through Him that my life has meaning. Malibu, USA November 2016

Vincent De Sapio HRL Laboratories

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:36:48, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.001

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:36:48, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.001

Notation

A specific set of notational standards is employed in this book. In the following sections we build up the notational standards that are used in the following chapters, starting with general mathematical objects and proceeding to kinematic objects, dynamical objects, and block diagram elements.

General Mathematical Objects Sets The following standard set notation is employed: tu | @ P K lK R Rn Rmˆn C H Sn

designation of a set such that for all element of orthogonal to orthogonal complement set of real numbers set of real n-dimensional vectors set of real m ˆ n matrices set of complex numbers set of quaternions set of points on an n-dimensional unit sphere

An example is as follows: ATλ K x, @λ P Rm and @x P kerpAq, where kerpAq “ tx P Rn | Ax “ 0u. This would read as follows: ATλ is orthogonal to x for all λ in the set of real mdimensional vectors and for all x in the kernel of A, where the kernel of A is the set of all real n-dimensional vectors, x, such that Ax “ 0.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

xviii

Notation

Scalars Scalars (rank 0 tensors) are represented with nonbold italic characters (e.g., a). These include scalars as well as scalar components of vectors and matrices. Scalar components of vectors and matrices are denoted with a subscripted index to the right of the scalar symbol (e.g., vi , Mi j ). The following standard operators are employed: δ d d˝

9 l

variation derivative time derivative

Complex Numbers and Quaternions Complex numbers and quaternions are represented with nonbold lowercase (typically) italic characters. The components can be expressed as a sum of the real and imaginary parts, for example, z “ a ` ib h “ h0 ` h1 i ` h2 j ` h3 k.

Vectors, Points, and Line Segments Vectors (rank 1 tensors) are represented with bold lowercase (typically) italic characters. Vectors can be expressed as a 1-dimensional array or as a linear combination of basis vectors, with indexed scalar components (displayed as nonbold italic characters). Basis vectors are denoted as e. ˆ An example follows: ¨ ˛ v1 3 ÿ ˝ vi eˆi “ v1 eˆ1 ` v2 eˆ2 ` v3 eˆ3 . v “ v2 ‚ “ i“1 v3 Points are represented using nonbold italic characters (e.g., A). Line segments Ý Ñ between two points are represented using an arrow (e.g., AB).

Matrices and Tensors Matrices (rank 2 tensors) are represented with bold uppercase (typically) italic characters. Matrices can be expressed as a 2-dimensional array or as a linear combination of dyads, with indexed scalar components (displayed as nonbold italic characters). Dyads consist of a pair of base vectors separated by an outer product symbol, b, for example, ˙ ÿ ˆ 2 2 ÿ M11 M12 “ Mi j eˆi b eˆ j “ M11 eˆ1 b eˆ1 ` M12 eˆ1 b eˆ2 M“ M21 M22 i“1 j“1 `M21 eˆ2 b eˆ1 ` M22 eˆ2 b eˆ2 . The identity matrix is denoted as 1 and the zero matrix is denoted as 0.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

Notation

xix

Vector and Matrix Operators The following standard vector and matrix operators are employed: ¨ xl, ly }l} ˆ b δ d ds

9 l

B ,∇ B˝

impq kerpq proj˝ pq T pq ¯ l

dot product inner product norm cross-product outer product variation derivative with respect to a scalar, s time derivative partial derivative, gradient image or range of a matrix kernel or null space of a matrix projection of a vector onto a subspace tangent space operator dynamically consistent (mass-weighted) inverse of a matrix

The partial derivative/gradient operators are overloaded for scalars and vectors. For example, given a scalar, U P R, and a vector, v P Rm , the respective gradients are ¨ ˛ BU

n ÿ ˚ Bq. 1 ‹ BU BU n ‹ “ ∇U “ eˆi “ ˚ ˝ .. ‚ P R Bq Bq i i“1 BU Bqn

and ¨ v∇ “

m ÿ n ÿ

Bv1 Bq1

˚ . Bvi Bv “ eˆi b eˆ j “ ˚ ˝ .. Bq i“1 j“1 Bq j

Bvm Bq1

¨¨¨ ..

.

¨¨¨

Bv1 Bqn

˛

.. ‹ mˆn ‹ . . ‚P R

Bvm Bqn

Kinematic Objects Objects having a kinematic meaning inherit all of the aforementioned rules with respect to their mathematical type. Additionally, they adhere to the following with regard to their physical type. A position vector, r, uses a right subscript to denote the material point it refers to and a left superscript to denote the basis it is expressed in. Velocity, v, and acceleration, a, vectors additionally denote the frame that motion is relative to using a “:” separator in the right subscript. Angular velocity, ω, and angular acceleration vectors, α, use a right subscript to denote the body they refer to and a left superscript to denote the basis they are expressed in. As with velocity, they additionally denote the frame that motion is relative to, using a “:”. Any annotation can be omitted if the information conveyed by it

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

xx

Notation

is already clear from context. Generalized coordinates are denoted as q and operational space coordinates are denoted as x. Coordinate transformation matrices, including both orthogonal rotation matrices, Q, and homogenous transformation matrices, T , denote the frame of interest using a left subscript and the embedding frame using a left superscript. Unit quaternions, h, use similar annotation. Jacobian matrices use a right subscript to denote the object (material point, body, etc.) they refer to and a left superscript to denote the basis they are expressed in. Again, any annotation can be omitted if the information conveyed by it is already clear from context.

Cartesian Space Quantities Ad

,G

dt O

O B dÝ Ñ AB B rGA B A rB B v GA:O B aGA:O A

ωA:O αA:O A Q B Qk pθ q A h B hk pθ q B B

A B B

T GA B A

center of mass point time derivative relative to O, expressed in A change relative to O, expressed in A displacement vector between points A and B, expressed in B position of center of mass, G, of body A expressed in B point on body B to which body A attaches, expressed in B velocity of center of mass, G, of body A, relative to O, expressed in B acceleration of center of mass, G, of body A, relative to O, expressed in B angular velocity of body A, relative to O, expressed in B angular acceleration of body A, relative to O, expressed in B rotation matrix of B with respect to A rotation matrix representing a spin of θ about axis k quaternion of B with respect to A quaternion representing a spin of θ about axis k screw displacement homogenous transformation matrix of B with respect to A Jacobian of position of center of mass, G, of body A expressed in B Jacobian of body A expressed in B

Configuration Space Quantities q

generalized coordinate vector

Constraint Space Quantities φ  C W ψ A

holonomic constraint vector (general zeroth-order constraints) holonomic constraint Jacobian matrix nonholonomic constraint matrix (linear first-order constraints) constraint null space matrix nonholonomic constraint vector (general first-order constraints) nonholonomic constraint matrix (linear second-order constraints)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

Notation

xxi

Task Space Quantities x J

task space coordinate vector task Jacobian matrix

Dynamic Objects Objects having a dynamical meaning inherit all of the aforementioned rules with respect to their mathematical type. Additionally, they adhere to the following with regard to their physical type. Translational momentum vectors, p, have the same scripting as velocity vectors. Angular momentum vectors, H , use the same scripting as angular velocity vectors. Additionally, angular momentum vectors denote the point about which they are evaluated using a right superscript. Inertia tensors, I, have scripting similar to angular momentum vectors. Again, any annotation can be omitted if the information conveyed by it is already clear from context.

Cartesian Space Quantities M g B A fB B A ϕB B pGA:O B

G

A H A:O

B GA A:O

I

mass of point or body acceleration due to gravity (e.g.,« 9.8m{s2 on earth) force that body A exerts on body B, expressed in B moment that body A exerts on body B, expressed in B translational momentum of center of mass, G, of body A, relative to O, expressed in B angular momentum of body A, relative to O, about center of mass, G, of body A, expressed in B inertia tensor of body A, relative to O, about center of mass, G, of body A, expressed in B

Configuration Space Quantities p τ M b g L H G S

generalized momentum vector generalized force vector generalized mass matrix generalized Coriolis-centrifugal vector generalized gravity vector Lagrangian Hamiltonian Gauss function Gibbs function

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

xxii

Notation

Constraint Space Quantities λ H α ρ

T

Lagrange multipliers (constraint forces) constraint space mass matrix constraint space Coriolis-centrifugal vector constraint space gravity vector constraint null space projection matrix

Task Space Quantities NT f  μ p

task null space projection matrix task space force vector task space mass matrix task space Coriolis-centrifugal vector task space gravity vector

Block Diagrams Block diagrams use a number of common schematic elements, as follows. For general (nonlinear) operators, a dashed line and an unfilled arrow are used to denote the input argument into the block: summation, z “ x ´ y ş integration, x “ x9 dt concatenation, z “ pxT yT qT linear operator, y “ Ax general (nonlinear) operator, y “ f pxq mixed operator, z “ Apxqy

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:40:01, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.002

1

Introduction

Dynamics is traditionally defined as the classical study of motion with respect to the physical causes of motion, that is, forces and moments. Kinematics, on the other hand, is concerned with the study of motion without respect to the underlying physical causes. In this sense, kinematics is really a fundamental prerequisite upon which dynamics is constructed. For the purposes of this text, the terms dynamics and mechanics are taken to be synonymous. The choice of which term is used is based more on the academic community than on a strict technical distinction. The engineering community typically adopts the term dynamics and the physics and applied mathematics communities typically adopt the term mechanics. The term dynamics is predominantly used in this text.

1.1

Historical Background Interest in the dynamics of linked multibody systems has existed throughout much of recorded human history. As an example, representations of the human form in art have included anthropomorphic constructions made up of mechanical elements like those depicted in Giovanni Braccelli’s Bizzarie di Varie Figure published in 1624 (see Figure 1.1). Braccelli’s art coincided with the birth of the mechanical philosophy of René Descartes, Pierre Gassendi, and others. The mechanical philosophy sought to describe physical phenomena in terms of intricate mechanisms. Decades after the birth of the mechanical philosophy, a systematic theory of mechanics began to flourish with Newtonian mechanics. Analytical dynamics (historically referred to as analytical mechanics) is identified with a number of formulations of classical mechanics that arose after Isaac Newton published his Philosophiae Naturalis Principia Mathematica in 1687. The cornerstones of Newtonian mechanics are his laws of motion, which were applied to point masses. From a modern perspective, Newton’s second law can be seen more generally as a conservation law, specifically, as a law of conservation of momentum. As such, Newtonian mechanics, and its extension to extended (rigid) bodies by Leonhard Euler, are based on two fundamental conservation principles: (1) the conservation of translational momentum and (2) the conservation of angular momentum.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

2

Introduction

Figure 1.1 Anthropomorphic forms adapted from plate no. 12 of Bizzarie di Varie Figure, 1624,

by Giovanni Battista Braccelli, Livorno, Library of Congress Lessing J. Rosenwald Collection. Braccelli’s Mannerist-styled work consists of a set of 50 etchings depicting anthropomorphic mechanical figures like this one. The human form has long been an inspiration in the study of mechanical systems.

In contrast to the conservation principles underlying the Newton-Euler mechanics, the analytical dynamics that followed were based on variational principles. The conservation of vector quantities like translational and angular momentum was replaced with principles rooted in the variation of scalar quantities like work and energy. An important precursor to the subsequent development of variational mechanics was the concept of the vis viva (living force) proposed by Gottfried Leibniz, a contemporary of Newton. The vis viva corresponds to our present notion of kinetic energy. In Leibniz’s mechanics, momentum was replaced by kinetic energy and force was replaced by work of the force (Lanczos 1986). Development of variational mechanics required a mathematical tool beyond the basic calculus of Newton and Leibniz. The calculus of variations, concerned with extremizing functionals (mappings of functions to scalar values), emerged as this tool. Johann Bernoulli was the first to exploit the calculus of variations in solving the brachistochrone curve problem. However, Euler is usually credited with the formal development of the calculus of variations in his 1744 Methodus inveniendi. Around the time of Euler’s Methodus inveniendi, in 1743, Jean Le Rond d’Alembert published his Traité de dynamique. This articulated d’Alembert’s Principle of Virtual Work. Although Johann Bernoulli is credited with first proposing the Principle of Virtual Work for cases of static equilibrium, d’Alembert is credited with extending the principle to dynamic equilibrium by interpreting the acceleration terms in Newton’s equation of motion as inertial forces. The principle is based on the notion of virtual

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

1.1 Historical Background

3

displacement, defined as an infinitesimal change of the system’s configuration coordinates while time is frozen. The displacement is virtual because no actual displacement occurs. Rather, a virtual displacement is used as a conceptual mechanism. D’Alembert’s Principle can be viewed as a zeroth-order variational principle, as it is based on the zeroth-order derivative of displacement. It should be noted that with the advent of analytical dynamics and variational principles like d’Alembert’s, the concept of generalized coordinates became relevant. As the name implies, these coordinates are a generalization of the Cartesian coordinates used in Newton-Euler mechanics, whereby any consistent set of parameters that uniquely describe the configuration of the system can be chosen. The vector space defined by these generalized coordinates forms the configuration space of the system. Following d’Alembert’s Principle, the monumental work of Joseph-Louis Lagrange (born Giuseppe Lodovico Lagrangia) resulted in what is now known as Lagrangian mechanics. Together with contributions from Euler and William Rowan Hamilton, the Euler-Lagrange equations emerged as a logical consequence of Hamilton’s Principle of Least Action. Disentangling the individual contributions of these three eminent mechanicians with respect to Lagrangian mechanics can be a bit tedious. Consequently, we will not proceed in chronological order when discussing this. Hamilton’s Principle of Least Action has been referred to as “the most direct and most natural transformation of d’Alembert’s into a minimum principle” (Lanczos 1986, p. 111). The principle states that the path of a system in configuration space during a time interval is such that the action is stationary under all path variations. The action is defined as the integral of the Lagrangian over the time interval, where the Lagrangian is defined as the difference between the kinetic and potential energies of the system. The Euler-Lagrange equations of motion for the system emerge directly from this principle. Lagrange published his seminal work, Mécanique analytique, in 1788, formalizing these ideas into what is now known as Lagrangian mechanics. Hamilton’s reformulation of Lagrangian mechanics, published in 1833, constitutes what is now known as Hamiltonian mechanics. This reformulation involves a transformation of the Euler-Lagrange equations from a set of second-order differential equations in the generalized coordinates to a set of first-order differential equations in the generalized coordinates and generalized momenta. The Hamiltonian is defined as a new invariant corresponding to the total energy. Hamilton’s equations written in terms of the Hamiltonian are known as Hamilton’s canonical equations. Up to this point, the historical flow of the variational principles of mechanics has followed the path from d’Alembert’s Principle to Hamilton’s Principle to the EulerLagrange equations and, finally, to Hamilton’s canonical equations. It was mentioned that d’Alembert’s Principle can be viewed as a zeroth-order variational principle. Holonomic constraints, which take the form of algebraic functions of the generalized coordinates and possibly time, can be inherently addressed by zeroth-order variational principles using the method of Lagrange multipliers. Higher-order variational principles have also been proposed. One of the advantages of higher-order variational principles is the ability to address nonholonomic constraints, which take the form of algebraic functions of the higher-order derivatives of the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

4

Introduction

Figure 1.2 Historical progression of key principles of analytical dynamics. The variational

principles, which form the basis of analytical dynamics, are comprised of a number of formulations that arose after Newton’s mechanics.

generalized coordinates (generalized velocities and accelerations) and possibly time. The first-order variational principle, published by Philip E. B. Jourdain in 1909, is based on the notion of virtual velocity. Virtual power assumes the role that virtual work assumes in d’Alembert’s Principle. Subsequent developments by Thomas Kane in 1961 essentially rediscovered Jourdain’s Principle. Kane’s approach extended Jourdain’s approach to rigid bodies and introduced quasi-velocities to implicitly handle nonholonomic constraints. Jourdain’s Principle was influenced by Carl Friedrich Gauss’s second-order variational principle, based on the notion of virtual accelerations, published in 1829. Gauss used the notion of virtual acceleration to establish a true minimum principle known as Gauss’s Principle of Least Constraint, which minimizes the quadratic form known as the Gauss function. Heinrich Rudolf Hertz reinterpreted a special case of Gauss’s Principle as the Principle of Least Curvature in 1894. Subsequent developments by Josiah Willard Gibbs (1879), Paul Appell (1900), and others led to the Gibbs-Appell equations, which have their lineage in Gauss’s Principle. The Gibbs-Appell equations,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

1.2 Devices That Illustrate Principles of Analytical Dynamics

5

like Kane’s equations, make use of quasi-variables – in this case quasi-accelerations – to implicitly handle nonholonomic constraints. Although they derive from variational principles of different order, the Gibbs-Appell equations and Kane’s equations can be viewed as identical. However, because the Gibbs-Appell equations are derived from a second-order variational principle, they can be stated in a concise form as the gradient of a scalar function, known as the Gibbs function, with respect to the quasi-accelerations. The Gibbs function can be related to the Gauss function. Figure 1.2 summarizes the historical progression of some of the key principles of analytical dynamics that are covered in this book. I have by no means presented an exhaustive history, and the interested reader is referred to Dugas’s excellent history of mechanics (Dugas 1988), which addresses ancient through modern developments in mechanics.

1.2

Devices That Illustrate Principles of Analytical Dynamics Before jumping into a formal exposition of analytical dynamics, we will look at some motivating examples. Toys and other objects of amusement tend to make the most compelling examples. The reader is encouraged to refer back to these when encountering similar detailed technical examples presented in the subsequent chapters. Figure 1.3 displays some devices composed of branching kinematic chains. A modified double pendulum (top), the Swinging Sticks Kinetic Energy Sculpture by BTS Trading GmbH, exhibits chaotic motion characterized by a sensitive dependence on initial conditions. As with all real-world mechanical systems, it dissipates energy; however, in this example, the double pendulum gives the illusion of perpetual motion through the use of electromagnetic coils mounted in the base and permanent neodymium rareearth magnets mounted in the arms. The electromagnets measure the speed of the rotating arms and impart additional kinetic energy into the system. The gimbaled Super Precision Gyroscope distributed by Gyroscope.com (bottom left) consists of a highspeed rotor mounted, in this case, on a two-axis gimbal. The rotor on a high-speed gyroscope is precisely balanced and mounted on low-friction bearings to demonstrate the conservation of angular momentum (see the example in Section 5.3.5). Similar to the Swinging Sticks Kinetic Energy Sculpture is the Chaos Machine by Fat Brain Toys, a reconfigurable tree-structured mechanism (bottom right). This simple device illustrates the complex motion characteristic of multilink kinematic chains. Figure 1.4 displays some devices that operate under holonomic constraints. These constraints are discussed in detail in later chapters. The holonomically constrained devices shown here all involve loop closures that can be represented as algebraic conditions on the configuration coordinates. The Falcon (top), by Novint Technologies Inc., is a haptic (force feedback) game controller based on the kinematics of the Delta parallel robot (Clavel 1991). Three translational degrees of freedom are provided by the kinematic structure of the Falcon, which uses four-bar parallelogram linkages in the three arms to maintain the fixed orientation of the end effector. Three actuators mounted in

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

6

Introduction

Figure 1.3 Some devices composed of branching kinematic chains. (Top) The Swinging Sticks

Kinetic Energy Sculpture by BTS Trading GmbH. (Bottom Left) A gimbaled Super Precision Gyroscope distributed by Gyroscope.com. (Bottom Right) The Chaos Machine by Fat Brain Toys. All images © 2016 Vincent De Sapio.

the base allow the device to provide force feedback to the user. The Hoberman Sphere (bottom left) is a collapsing spherical structure made up of six rings, each of which comprises a series of connected four-bar parallelogram linkages that produce scissorlike motion. The overall structure has 1 degree of freedom and is able to radially expand and contract. A Stirling engine (bottom right), produced by Wiggers Stirling HeiBluft Modellbau, makes use of closed chain slider-crank and four-bar linkages to convert reciprocating piston motion into rotational motion of a flywheel. Figure 1.5 displays some devices that operate under nonholonomic constraints. As with holonomic constraints, we discuss nonholonomic constraints in detail in later chapters. The nonholonomically constrained devices shown here all involve rolling/spinning constraints that can be represented as algebraic conditions on the configuration

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

1.2 Devices That Illustrate Principles of Analytical Dynamics

7

Figure 1.4 Some devices that operate under holonomic constraints. (Top) The Falcon haptic game

controller by Novint Technologies Inc. (Bottom Left) The Hoberman Sphere © 1993 Charles Hoberman. (Bottom Right) A Stirling engine by Wiggers Stirling HeiBluft Modellbau. All images © 2016 Vincent De Sapio.

velocities. Specifically, the no-slip rolling/spinning condition requires zero velocity of the instantaneous material contact point of the device with the external surface. The Euler’s Disk (top), distributed by Toysmith, illustrates the dynamics of a rolling/spinning disk on a flat surface. The complex motion of the disk produces intricate traces, which can be investigated in simulation (see an example in Section 6.2.5). Ollie (bottom left), by Sphero Inc., is a two-wheeled robot controlled by a smartphone app. The wheels are independently driven to allow steering and maneuvering. A gyroscope and accelerometer are incorporated into the control unit for inertial sensing. Sphero SPRK edition (bottom right), by Sphero Inc., is a spherical robot also controlled by a smartphone app. Internal drive wheels and a stabilizer provide forward propulsion and maneuvering. As with Ollie, a gyroscope and accelerometer are incorporated into the control unit for inertial sensing.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

8

Introduction

Figure 1.5 Some devices that operate under nonholonomic constraints. (Top) Euler’s Disk by

Toysmith. (Bottom Left) Ollie by Sphero Inc. (Bottom Right) Sphero SPRK edition by Sphero Inc. All images © 2016 Vincent De Sapio.

1.3

Scope of This Book This book is intended to cover the foundations of analytical dynamics of discrete systems. By discrete systems, we mean systems made up of a discrete set of point masses or rigid bodies, as opposed to continuous systems. Continuous systems are the subject of continuum mechanics, which addresses infinite-dimensional deformable bodies, that is, bodies with infinite degrees of freedom. The study of flexible multibody systems, which include both rigid and deformable bodies, is an active area of research. Although such systems are outside the scope of the main body of this book, the appendix addresses some of the basics of the application of continuum mechanics to flexible multibody systems. The material covered in this book is intended for an an intermediate to advanced graduate-level audience. As such, it is not intended as an introductory book on dynamics or classical mechanics. Some of the topics covered, particularly higher-order variational principles, are not commonly covered in engineering dynamics texts. Preceding the material on dynamics is a chapter providing a brief mathematical background in

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

1.3 Scope of This Book

9

linear algebra, vectors and tensors, differential geometry, and optimization. A chapter on the kinematics of discrete systems directly precedes the chapters devoted to dynamics. Conservation principles are then addressed, followed by variational principles. Separate chapters cover zeroth-, first-, and second-order principles. The variational principles are based on a configuration space description comprising generalized coordinates. After addressing these configuration space formulations, we present a chapter based on an alternate, task space formulation of dynamics using task coordinates. The following chapter presents applications to biomechanical systems. Such systems are an active area of study, and this chapter is intended to provide a brief introduction to system-level modeling of musculoskeletal and neuromuscular dynamics. The final chapter provides a short survey of some analytical dynamics software. This includes examples of general purpose mathematical software as well as dedicated multibody dynamics software. The subsequent chapters provide example problems that are worked out in detail. The intention is to give the reader exposure to systematic approaches to applying the concepts presented to practical examples, thereby reinforcing abstract concepts with concrete applications.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:22:57, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.003

2

Mathematical Preliminaries

This chapter is intended to present only a brief exposition of the mathematics helpful for understanding the material presented in the subsequent chapters. The reader is also encouraged to consult full texts in the mathematical subjects addressed here, if needed. Linear algebra, including coverage of vectors and tensors, basic differential geometry, and static optimization, will be covered. Again, these sections are not complete expositions in these subjects but are intended to provide the reader with sufficient mathematical background.

2.1

Linear Systems Linear systems are mathematical models of a system, physical or otherwise, that exclusively employ linear operators. While the models addressed in analytical dynamics are typically highly nonlinear, methods associated with linear systems are indispensable in the analysis of dynamical systems. Linear algebra forms the fundamental theory for analyzing linear systems. Vector spaces and linear mappings between vector spaces are of fundamental importance. We will start by addressing the general properties of n-dimensional vector spaces, Rn , and then address the more concrete case of vectors and tensors in R3 .

2.1.1

Vector Spaces in Rn A vector or linear space is a collection of objects, namely, vectors, for which addition and multiplication by scalars is defined.

Inner Product For our purposes, all vector spaces that we will be concerned with possess an inner product and therefore are also termed inner product spaces. d e f i n i t i o n 2.1 The inner product ,  satisfies the following properties: 1. 2. 3. 4.

u + v, w = u, w + v, w. αu, v = α u, v. u, v = v, u. u, u ≥ 0.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.1 Linear Systems

11

Figure 2.1 Triangle ABC demonstrating the law of cosines, c2 = a2 + b2 − 2ab cos C.

The inner product is a generalization of the dot product and for Euclidean space is equal to the dot product. That is, u, v = u · v =

n 

ui vi .

(2.1)

i=1

Next, we note a basic property of triangles, the law of cosines: t h e o r e m 2.2 Given a triangle, ABC, the following relationship holds: c2 = a2 + b2 − 2ab cos C.

(2.2)

This is known as the law of cosines. Proof The triangle ABC can be split into two right triangles (Figure 2.1). We thus have a2 = x2 + h2

(2.3)

c2 = (b − x)2 + h2 = b2 + x2 − 2bx + h2 .

(2.4)

and

Substituting h2 = a2 − x2 into (2.4), we have c2 = b2 + x2 − 2bx + a2 − x2 = a2 + b2 − 2bx.

(2.5)

Noting that x = a cos C, we obtain the result c2 = a2 + b2 − 2ab cos C.

(2.6)

 From the law of cosines we can deduce the following geometric property of the inner product. t h e o r e m 2.3 The inner product of a vector a with a vector b is given by a, b = a b cos θ .

(2.7)

Proof By the law of cosines, we have b − a2 = a2 + b2 − 2 a b cos θ ,

(2.8)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

12

Mathematical Preliminaries

or b − a, b − a = a, a + b, b − 2 a b cos θ .

(2.9)

b − a, b − a = a, a + b, b − 2 a, b ,

(2.10)

Noting that

we can equate (2.9) and (2.10). Eliminating terms yields the result a, b = a b cos θ .

(2.11)



Basis of a Vector Space To describe a vector space, it is convenient to define a basis which can represent any vector in the vector space. d e f i n i t i o n 2.4 A set of vectors, {v 1 , . . . , v n }, forms a basis of a vector space, V , if the vectors span V and are linearly independent. t h e o r e m 2.5 Let a vector space, V , have a basis, B, consisting of a set of vectors, {v 1 , . . . , v n }. Any vector x ∈ V may be expressed as a unique linear combination of the basis vectors x = c1 v 1 + c2 v 2 + · · · + cn v n .

(2.12)

Proof Since {v 1 , . . . , v n } is a basis of V it spans V . Consequently, there is at least one linear combination of the basis vectors that expresses x, namely, c1 v 1 + c2 v 2 + · · · + cn v n . Let us assume that there is another linear combination of the same basis vectors, x = d1 v 1 + d2 v 2 + · · · + dn v n .

(2.13)

x − x = 0 = (c1 − d1 )v 1 + (c2 − d2 )v 2 + · · · + (cn − dn )v n .

(2.14)

Then,

Since {v 1 , . . . , v n } is a basis, it is linearly independent. As such, all of the coefficients, (ci − di ), must be zero. Therefore, c1 = d1 , c2 = d2 , . . . , cn = dn , and the set of coefficients is unique.

(2.15)



For any vector, x, the set of coefficients, {c1 , . . . , c2 }, associated with the unique linear combination of basis vectors are referred to as the coordinates (or components) of x with respect to the given basis. For a basis, B, consisting of vectors, {v 1 , . . . , v n }. The coordinate vector of x = c1 v 1 + c2 v 2 + · · · + cn v n ,

(2.16)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.1 Linear Systems

13

with respect to B, is expressed as ⎞ c1 ⎜ ⎟ B x = ⎝ ... ⎠ . ⎛

(2.17)

cn So, we have the following relationships: ⎛

↑ ⎝ x = v1 ↓

↑ ... ↓

⎞⎛c ⎞ 1 ↑ ⎜ .. ⎟ ⎠ v n ⎝ . ⎠ = S Bx ↓ cn

(2.18)

and B

x = S−1 x,

(2.19)

where ⎛

↑ ⎝ S = v1 ↓

↑ ... ↓

⎞ ↑ vn ⎠ . ↓

(2.20)

Example: Given a basis, B, in R3 consisting of {v 1 , v 2 , v 3 }, where ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 2 1 1 v 1 = ⎝ 1 ⎠ , v 2 = ⎝ 2 ⎠ , v 3 = ⎝ 1/2 ⎠ , 1/2 1/2 2

(2.21)

S is constructed as ⎛

↑ S = ⎝ v1 ↓

↑ v2 ↓

⎞ ⎛ 2 ↑ v3⎠ = ⎝ 1 ↓ 1/2

1 2 1/2

⎞ 1 1/2⎠ . 2

(2.22)

Given the vector x expressed as the linear combination (see Figure 2.2) x = 2v 1 + 3v 2 + 1v 3 , we have the following coordinate vector, with respect to B: ⎛ ⎞ 2 B x = ⎝3⎠. 1 We compute x with respect to the standard basis, as ⎛ ⎞⎛ ⎞ ⎛ ⎞ 2 1 1 2 8 x = S Bx = ⎝ 1 2 1/2⎠ ⎝ 3 ⎠ = ⎝ 81/2 ⎠ . 41/2 1/2 1/2 2 1

(2.23)

(2.24)

(2.25)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

14

Mathematical Preliminaries

Figure 2.2 Vector coordinates with respect to a basis, B, illustrated as a linear combination of

basis vectors, {v 1 , v 2 , v 3 }.

Conversely, having started with x, we would compute Bx as ⎛

2 B x = S−1 x = ⎝ 1 1/2

1 2 1/2

⎞−1 ⎛ ⎞ ⎛ ⎞ 1 8 2 1/2⎠ ⎝ 81/2 ⎠ = ⎝ 3 ⎠ . 41/2 2 1

(2.26)

Given a linear transformation τ(x) = Ax,

(2.27)

from Rn to Rn we may be interested in expressing the corresponding transformation with respect to some basis, B: B

τ(x) = BBx.

(2.28)

The matrix B is determined in the following manner. First, let a vector x be expressed as a linear combination of basis vectors {v 1 , . . . , v n }, x = c1 v 1 + · · · + cn v n . The corresponding coordinate vector with respect to B is ⎛ ⎞ c1 ⎜ .. ⎟ B x = ⎝ . ⎠.

(2.29)

(2.30)

cn By linearity of τ(x), we have τ(x) = τ(c1 v 1 + · · · + cn v n ) = c1 τ(v 1 ) + · · · + cn τ(v n ).

(2.31)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

15

2.1 Linear Systems

Now, {τ(v 1 ), . . . , τ(v n )} can be expressed as linear combinations of {v 1 , . . . , v n }: τ(v 1 ) = B11 v 1 + · · · + Bn1 v n , .. .

(2.32)

τ(v n ) = B1n v 1 + · · · + Bnn v n , where the coordinate vectors, {Bτ(v 1 ), . . . , Bτ(v n )}, are ⎞ ⎞ ⎛ ⎛ B11 B1n ⎟ ⎟ ⎜ ⎜ B τ(v 1 ) = ⎝ ... ⎠ , . . . , Bτ(v n ) = ⎝ ... ⎠ . Bn1

(2.33)

Bnn

We now have the following expression for τ(x): τ(x) = c1 (B11 v 1 + · · · + Bn1 v n ) + · · · + cn (B1n v 1 + · · · + Bnn v n )

(2.34)

τ(x) = (c1 B11 + · · · + cn B1n )v 1 + · · · + (c1 Bn1 + · · · + cn Bnn )v n .

(2.35)

or

So, the coordinate vector, Bτ(x), is ⎛ c1 B11 + · · · + cn B1n ⎜ .. B τ(x) = ⎝ .

⎞

⎛

B11 ⎜ .. B=⎝ . Bn1

... .. . ...

... .. . ...

B11 ⎟ ⎜ .. ⎠=⎝ .

c1 Bn1 + · · · + cn Bnn and

⎛

Bn1

⎞ ⎛ B1n ↑ .. ⎟ = ⎝ B τ(v 1) . ⎠ ↓ Bnn

⎞⎛ ⎞ B1n c1 .. ⎟ ⎜ .. ⎟ = BBx, . ⎠⎝ . ⎠ Bnn

↑ ... ↓

(2.36)

cn ⎞ ↑ B τ(v n )⎠ . ↓

(2.37)

To relate A and B, we first note that SBτ(x) = SBBx

(2.38)

x = S−1 x.

(2.39)

SBτ(x) = SBS −1 x.

(2.40)

τ(x) = SBτ(x) = SBS−1 x = Ax.

(2.41)

and B

So,

Finally,

So, A = SBS−1

and,

B = S−1 AS.

(2.42)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

16

Mathematical Preliminaries

Algorithm 1 Gram-Schmidt process The Gram-Schmidt process allows us to construct an orthonormal basis from an arbitrary basis. Given a subspace, V , of Rn with some basis, {v 1 , . . . , v m }, we can construct an orthonormal basis, {u1 , . . . , um }, for V as follows: 1 1: u1 = v  v 1 {Establish u1 by normalizing v 1 } 1 2:

for i = 2 to m do

3:

Vi−1 = span(u1 , . . . , ui−1 )

4:

projVi−1 v i = (u1 · v i )u1 + · · · + (ui−1 · v i )ui−1

5:

ui =

6:

end for

1 (v i v i −projVi−1 v i

− projVi−1 v i )

d e f i n i t i o n 2.6 The orthogonal complement, V ⊥ , of a vector space, V , is the set of those vectors, x, in Rn that are orthogonal to all vectors in V : V ⊥ = {x|x ∈ Rn , v · x = 0, ∀v ∈ V } .

(2.43)

Given a basis, {v 1 , v 2 , . . . , v m }, V ⊥ can be equivalently defined as V ⊥ = {x|x ∈ Rn , v i · x = 0, for i = 1, . . . , m} .

(2.44)

d e f i n i t i o n 2.7 Consider a subspace, V , of Rn with orthonormal basis, {u1 , . . . , um }. For any vector, x, in Rn , there is a unique vector, w, in V such that x − w is in V ⊥ . The vector, w, is called the orthogonal projection of x onto V and is given by projV x = (u1 · x)u1 + · · · + (um · x)um .

(2.45)

Given an orthonormal basis, {u1 , . . . , un }, of Rn , x = (u1 · x)u1 + · · · + (un · x)un ,

(2.46)

for all x in Rn . The Gram-Schmidt process described in Algorithm 1 employs orthogonal projection to generate an orthonormal basis from an arbitrary basis.

Least Squares We consider the over determined system Ax = y,

(2.47)

where A ∈ Rm×n is full rank and m > n. In this case the vector, y, is not generally in the image of A, so the equality in (2.47) does not generally hold. However, if we define the vector space, V , V = im(A),

(2.48)

the projection of y onto V is in the image of A. So, Ax∗ = projV y

(2.49)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.1 Linear Systems

17

for some vector x∗ . Furthermore, the residual, or difference between the observed data, y, and the estimate, projV y, is orthogonal to V : (y − projV y)⊥V

(2.50)

(y − Ax∗ )⊥im(A).

(2.51)

ker(AT ) = im(A)⊥ ,

(2.52)

(y − Ax∗ ) ∈ ker(AT ).

(2.53)

AT (y − Ax∗ ) = 0,

(2.54)

AT Ax∗ = AT y.

(2.55)

or

Noting that

we have

So,

and

Thus our least squares solution that minimizes the residual between the observed data, y, and the estimate, projV y, is x∗ = (AT A)−1 AT y.

(2.56)

We define the least squares left inverse of A as AI  (AT A)−1 AT .

(2.57)

Least Norm We now consider the under determined system Ax = y,

(2.58)

where A ∈ Rm×n is full rank and m < n. In this case, there is an infinite number of solutions, x, for a given y in (2.58). Our solution is x = xP + xN ,

(2.59)

where xP is a particular solution and xN ∈ ker(A) are the null space solutions associated with y = 0. Since there is an infinite number of particular solutions to (2.58), we can find a specific one that minimizes some quantity. For example, we may be interested in

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

18

Mathematical Preliminaries

the solution that minimizes the norm of x. In this case the particular solution will be orthogonal to all null space solutions, xP ⊥xN .

(2.60)

im(AT ) = ker(A)⊥ .

(2.61)

We note that

So, xP ∈ im(AT ),

or

xP = AT v,

(2.62)

for some vector, v. Thus, Ax = AxP = AAT v = y,

(2.63)

v = (AAT )−1 y.

(2.64)

and

Our solution that minimizes the norm of x is then x = xP = AT (AAT )−1 y.

(2.65)

We define the least norm right inverse of A as A†  AT (AAT )−1 .

(2.66)

All solutions to Ax = y are given by x = A† y + xN ∀xN ∈ ker(A).

(2.67)

See Figure 2.3 for a graphical depiction of the space of solutions.

Eigenvalue Problem d e f i n i t i o n 2.8 Consider an eigenvalue, λ, of an n × n matrix, A. The kernel of λ1n − A is called the eigenspace of A associated with λ and is denoted by Eλ : Eλ = ker(λ1n − A).

(2.68)

d e f i n i t i o n 2.9 Consider an n × n matrix, A. A basis of Rn consisting of eigenvectors of A is called an eigenbasis for A. The eigenbasis spans the eigenspace of A.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.1 Linear Systems

19

Figure 2.3 Least norm solution, xP , and space of all solutions, x. All solutions are separated by a null space vector, xN ∈ ker(A).

Given an eigenbasis, E = {v 1 , . . . , v n }, of a matrix, A, we have the following transformation with respect to E: ⎞ ⎛ λ1 0 · · · 0 ⎛ ⎞ ↑ ↑ ↑ ⎜ 0 λ2 0⎟ ⎟ ⎜ (2.69) D = S−1 AS = ⎝ Bτ(v 1 ) · · · Bτ(v n )⎠ = ⎜ . .. ⎟ , . . . ⎝ . . .⎠ ↓ ↓ ↓ 0 0 · · · λn where D is a diagonal matrix with the eigenvalues, {λ1 , . . . , λn }, as the diagonal entries. The matrix S is a matrix with the eigenvectors, {v 1 , . . . , v n }, as the columns, ⎛ ⎞ ↑ ↑ ↑ S = ⎝ v1 · · · vn⎠ . (2.70) ↓ ↓ ↓ d e f i n i t i o n 2.10 An n × n matrix, A, is positive definite if all of its eigenvalues are positive. That is, λi > 0, for i = 1, . . . , n.

2.1.2

Vectors in R3 Given some coordinate frame, A, associated with an orthonormal basis, eˆAi , we can define a vector, v, as a linear combination of basis vectors: v=

3 

vi eˆAi = v1 eˆA1 + v2 eˆA2 + v3 eˆA3 .

(2.71)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

20

Mathematical Preliminaries

The components of this vector, expressed in A, can be represented as the coordinate vector A

v=

3 

vi AeˆAi = v1 AeˆA1 + v2 AeˆA2 + v3 AeˆA3

i=1

⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 1 0 0 v1 = v1 ⎝ 0 ⎠ + v2 ⎝ 1 ⎠ + v3 ⎝ 0 ⎠ = ⎝ v2 ⎠ . v3 0 0 1

(2.72)

Given another coordinate frame, B, associated with the orthonormal basis vectors, {eˆB1 , eˆB2 , eˆB3 }, we have the following relationship: eˆAi =

3 

(eˆAi · eˆB j )eˆB j .

(2.73)

j=1

Vector, v, can now be expressed as a linear combination of {eˆB1 , eˆB2 , eˆB3 }: v=

3  3 

vi (eˆAi · eˆB j )eˆB j .

(2.74)

i=1 j=1

The components of eˆAi , expressed in B, can be represented as the coordinate vector B

eˆAi =

3 

(eˆAi · eˆB j )BeˆB j

j=1

⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 1 0 0 ⎝ ⎠ ⎝ ⎠ ⎝ = (eˆAi · eˆB1 ) 0 + (eˆAi · eˆB2 ) 1 + (eˆAi · eˆB3 ) 0 ⎠ 0 0 1 ⎛ ⎞ eˆAi · eˆB1 = ⎝ eˆAi · eˆB2 ⎠ . eˆAi · eˆB3

(2.75)

The components of v expressed in B are then B

v=

3 

vi BeˆAi =

i=1

⎛

eˆA1 · eˆB1 = ⎝ eˆA1 · eˆB2 eˆA1 · eˆB3

3  3  i=1 j=1

eˆA2 · eˆB1 eˆA2 · eˆB2 eˆA2 · eˆB3

vi (eˆAi · eˆB j )BeˆB j ⎞⎛ ⎞ v1 eˆA3 · eˆB1 eˆA3 · eˆB2 ⎠ ⎝ v2 ⎠ , eˆA3 · eˆB3 v3

(2.76)

or ⎛

↑ B v = ⎝ BeˆA1 ↓

↑ B eˆA2 ↓

⎞⎛ ⎞ v1 ↑ B eˆA3 ⎠ ⎝ v2 ⎠ = BAQAv, ↓ v3

(2.77)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.1 Linear Systems

where the rotation matrix, Q, is defined as ⎛ ↑ B B ⎝ Q = e ˆA1 A ↓

2.1.3

↑ B eˆA2 ↓

⎞ ↑ B eˆA3 ⎠ . ↓

21

(2.78)

Tensors in R3 The outer product of two vectors, a and b, is denoted by a ⊗ b. Given the components of a and b expressed in some coordinate frame, C, we can compute the vector outer product as ⎛ ⎛ ⎞ ⎞ a1 b1 a1 b2 a1 b3 a1

 C a ⊗ Cb = ⎝ a2 ⎠ b1 b2 b3 = ⎝ a2 b1 a2 b2 a2 b3 ⎠ . (2.79) a3 a3 b1 a3 b2 a3 b3 A dyad, eˆi ⊗ eˆ j , involves the vector outer product of two basis vectors. We can define a dyadic (rank 2 tensor), T , as a linear combination of dyads: T =

3  3 

Ti j eˆAi ⊗ eˆA j = T11 eˆA1 ⊗ eˆA1 + T12 eˆA1 ⊗ eˆA2 + · · · + T33 eˆA3 ⊗ eˆA3 . (2.80)

i=1 j=1

The components of this tensor, expressed in A, can be represented in matrix form as ⎛ ⎞ ⎛ ⎞ 1 0 0 0 1 0 3  3  A T= Ti j AeˆAi ⊗ AeˆA j = T11 ⎝ 0 0 0⎠ + T12 ⎝ 0 0 0⎠ i=1 j=1 0 0 0 0 0 0 ⎛ ⎞ 0 0 0 ⎝ + · · · + T33 0 0 0⎠ , (2.81) 0 0 1 or

⎛

T11 A T = ⎝ T21 T13

T12 T22 T23

⎞ T13 T23 ⎠ . T33

(2.82)

The components of T expressed in B are B

T=

3  3  i=1 j=1

=

⎛

↑ Ti j ⎝ BeˆA1 j=1 ↓

3  3  i=1

Ti j BeˆAi ⊗ BeˆA j ⎛ ⎞ ← ↑ ⎜ B eˆA3 ⎠ AeˆAi ⊗ AeˆA j ⎝ ← ↓ ←

↑ B eˆA2 ↓

eˆTA1 B T eˆA2 B T eˆA3 B

⎞ → ⎟ →⎠ . →

(2.83)

So, we have the following transformation law for rank 2 tensors: B

T = BAQATBAQT .

(2.84)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

22

Mathematical Preliminaries

2.1.4

Operations on Vectors and Tensors The del operator is defined as n  ∂ ∇= eˆi ∂r i i=1

(2.85)

Given a scalar field, φ(r), a vector field, v(r) = ni=1 vi eˆi , and a tensor field, T (r) = m n i=1 j=1 Ti j eˆi ⊗ eˆ j , we define a set of operations. The gradient of a scalar field involves the direct product with the del operator, that is, ∇ ⊗ φ. The symbol, ⊗, will be omitted for conciseness, so grad(φ) = ∇φ =

n  ∂φ i=1

∂ri

(2.86)

eˆi

Similarly, the gradient of a vector field is given by grad(v) = v∇ =

m  n 

  ∂vi ∂ eˆ j = eˆi ⊗ eˆ j ∂r j ∂r j i=1 j=1 m

vi eˆi ⊗

i=1 j=1

n

(2.87)

The divergence of a vector field is given by div(v) = ∇ · v = v · ∇ =

n  i=1

 ∂vi ∂ = ∂ri ∂ri i=1 n

vi

(2.88)

The divergence of a tensor field is given by div(T ) = T · ∇ =

m  n  i=1 j=1

  ∂Tji ∂ eˆi = eˆi ∂r j ∂r j i=1 j=1 m

Tji

n

(2.89)

The curl is given by curl(v) = ∇ × v =

l  m  n 

i jk

i=1 j=1 k=1

∂vk eˆi ∂r j

(2.90)

The Laplacian of a scalar field is given by lap(φ) = ∇ 2 φ = (∇ · ∇ )φ =

n  ∂φ ∂ri ∂ri i=1

(2.91)

The Laplacian of a vector field is given by lap(v) = ∇ 2 v = (∇ · ∇ )v =

m  n  i=1 j=1

∂ vi eˆi ∂r j ∂r j

(2.92)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.2 Differential Geometry

2.2

23

Differential Geometry Differential geometry applies differential and integral calculus and linear algebra to the study of geometric problems. We will start off addressing the general properties of kmanifolds in Rn and then will address the more concrete case of curves and surfaces in R3 .

2.2.1

k-Manifolds in Rn A manifold is a space of points that is locally a Euclidean space near each point. Furthermore, a parameterization, α(u), of a manifold, M, is a map from a local coordinate space, U , to M. That is, {α|U → M}, where U = Rk and M ⊆ Rn (k ≤ n). The parameter, u, is the local coordinate vector, u = ( u1 . . . uk )T ∈ U . The dimension of the manifold is k, and the dimension of the ambient space is n. d e f i n i t i o n 2.11 A diffeomorphism is a map between manifolds that is differentiable and has a differentiable inverse. So, the map {g|M1 → M2 } is a diffeomorphism if it is differentiable and there is a differentiable function {g−1 |M2 → M1 }. Given a parameterization, α1 (u), for M1 and a parameterization, α2 (u), for M2 , we have α2 (u) = g(α1 (u)) = (g ◦ α1 )(u)

(2.93)

α1 (u) = g−1 (α2 (u)) = (g−1 ◦ α2 )(u).

(2.94)

and

2.2.2

Curves in R3 We now address space curves, which are 1-dimensional manifolds in R3 . Space curves can be represented in parameterized form as ⎛ ⎞ x(t ) r(t ) = ⎝ y(t ) ⎠ . (2.95) z(t ) The derivative, r (t ), is tangent to r(t ). If the parameter, t, is taken to be time, then r (t ) = v(t ) is the velocity along the curve. The differential arc length is given by (2.96) ds = dr = r (t )dt = r (t ) dt. The arc length over a region of the curve is

s(t ) =

t f ds =

 r (t ) dt.

(2.97)

to

For a curve parameterized by arc length, we have ds/dt = r (t ) = 1.

(2.98)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

24

Mathematical Preliminaries

The extrinsic curvature of a curve is given by the magnitude of the change in the unit tangent vector, T (t ) = r (t )/ r (t ) , with respect to change in arc length. That is, dT dT dt T  (t ) = = k= . (2.99) r (t ) ds dt ds If the curve is parameterized by arc length, then T (s) = r (s), r (s) = 1, and (2.100) k = r (s) .

2.2.3

Surfaces in R3 Surfaces are 2-dimensional manifolds in R3 . An implicit description of a surface, S, is defined as the zero set of a function, f (x, y, z), S = {x, y, z| f (x, y, z) = 0},

(2.101)

ˆ → S2 }, is given by or S = f −1 (0). The Gauss map, {n|S nˆ =

∇f . ∇f 

(2.102)

Alternately, given a parameterization of S, we have ⎛ ⎞ x(u, v) r(u, v) = ⎝ y(u, v) ⎠ .

(2.103)

z(u, v)

The Gauss map is then given by nˆ =

ru × rv , ru × rv 

(2.104)

where the right subscript is introduced as a shorthand for the partial derivative. For example, ru =

∂r . ∂u

(2.105)

The differential of the parameterization of (2.103) is dr = ru du + rv dv.

(2.106)

R = {ru , rv },

(2.107)

Defining the basis,

the coordinate vector of dr in R is

 R

dr =

du dv

 .

(2.108)

The differential of the Gauss map is d nˆ = nˆ u du + nˆ v dv.

(2.109)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.2 Differential Geometry

25

The partial derivatives can be expressed in the R basis, nˆ u = K11 ru + K21 rv

(2.110)

nˆ v = K12 ru + K22 rv .

(2.111)

d nˆ = (K11 ru + K21 rv )du + (K12 ru + K22 rv )dv,

(2.112)

d nˆ = (K11 du + K12 dv)ru + (K21 du + K22 dv)rv .

(2.113)

Thus,

or

The coordinate vector of d nˆ in R is then     K11 du + K12 dv du R = RK d nˆ = = RK Rdr, K21 du + K22 dv dv

(2.114)

where RK is the curvature tensor of the surface. The eigenvalues of RK are the principal curvatures, κ1 and κ2 . The Gaussian curvature is given by K = det(K ) = κ1 κ2 ,

(2.115)

and the mean curvature is given by 1 κ1 + κ2 tr(K ) = . 2 2 We can determine the elements, Ki j , by manipulating (2.110) and (2.111): H=

(2.116)

nˆ u , rv  = K11 ru , rv  + K21 rv , rv  ,

(2.117)

nˆ v , ru  = K12 ru , ru  + K22 rv , ru  ,

(2.118)

nˆ u , ru  = K11 ru , ru  + K21 rv , ru  ,

(2.119)

nˆ v , rv  = K12 ru , rv  + K22 rv , rv  .

(2.120)

Now n, ˆ ru  = n, ˆ rv  = 0, since ru and rv are orthogonal to n. ˆ So, n, ˆ ruu  = 0, ˆ ru u = nˆ u , ru  + n,

(2.121)

n, ˆ ru v = nˆ v , ru  + n, ˆ ruv  = n, ˆ rv u = nˆ u , rv  + n, ˆ rvu  = 0,

(2.122)

n, ˆ rv v = nˆ v , rv  + n, ˆ rvv  = 0.

(2.123)

For conciseness we will define a  nˆ u , ru  = − n, ˆ ruu  ,

(2.124)

ˆ ruv  = nˆ u , rv  = − n, ˆ rvu  , b  nˆ v , ru  = − n,

(2.125)

ˆ rvv  , c  nˆ v , rv  = − n,

(2.126)

A  ru , ru  ,

(2.127)

B  ru , rv  = rv , ru  ,

(2.128)

C  rv , rv  .

(2.129)

and

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

26

Mathematical Preliminaries

Equations (2.117) through (2.120) can be written as

or



b = K11 B + K21C,

(2.130)

b = K12 A + K22 B,

(2.131)

a = K11 A + K21 B,

(2.132)

c = K12 B + K22C,

(2.133)

  a b K11 = K12 b c

K21 K22



 A B . B C

(2.134)

So, 

K11 K12

K21 K22





 −1 a b A B = b c B C   

1 = AC − B2 and

a b b c

 R

K =

K11 K21

C −B

K12 K22

−B A

 =

1 = AC − B2

1 AC − B2





aC − bB bC − cB

aC − bB bA − aB



bA − aB , cA − bB

 bC − cB . cA − bB

(2.135)

(2.136)

Geodesics on Surfaces A tangent vector field, V , on a surface, S, maps to each point on S a vector, v, which is tangent to S at that point. That is, v(u, v) = v1 (u, v)ru + v2 (u, v)rv .

(2.137)

Let v(t ) be the restriction of the tangent vector field, V , to a curve, r(t ), sitting on the surface, S, with tangent space Tr (S) at a point r. The covariant derivative of v(t ) with reference to S is given by   d D v(t ) = projT v(t ) , (2.138) dt dt where projT () denotes the projection of a vector onto the tangent space of S. The vector field V is said to be parallel if D v(t ) = 0. dt The intrinsic geodesic curvature of a curve is D dr

 = proj r (s) kg = T ds ds if the curve is parameterized by arc length, or    T (t ) , kg = projT r (t )

(2.139)

(2.140)

(2.141)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

27

2.2 Differential Geometry

Figure 2.4 Relationship between extrinsic, geodesic, and normal curvature. The total curvature

vector is projected onto the tangent and normal spaces of the surface, yielding the geodesic and normal curvature vectors, respectively.

if not. The normal curvature is given as    T (t ) . proj kn = N r (t )

(2.142)

We have the following vector relationship:       T  (t ) T (t ) T (t ) = projN + projT . r (t ) r (t ) r (t )

(2.143)

These vectors form a right triangle. The curvatures are the corresponding magnitudes in this vector relationship and are related by k 2 = kn2 + kg2 .

(2.144)

This is depicted in Figure 2.4. A geodesic is the shortest path between two points on a surface. Geodesics correspond to curves with zero geodesic curvature (kg = 0; see Figure 2.5). They can be computed by solving a set of second-order nonlinear differential equations. If we parameterize our surface as r(u, v) ∈ R3 , the differential equations are given by (Do Carmo 1976) 1  1   1  u + 11 u + 2 12 u v + 22 v =0 2



v +

2 2

11 u

2

+

2   2 12 uv

+

2 2

22 v

=0,

(2.145) (2.146)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

28

Mathematical Preliminaries

Figure 2.5 Geodesics correspond to curves with zero geodesic curvature. A geodesic is illustrated. Another, nongeodesic curve is shown for comparison.

where the Christoffel symbols, , are found by solving the linear algebraic system 1 2 ru , ru  + 11 ru , rv  = ruu , ru  ,

11

(2.147)

1 2 ru , rv  + 11 rv , rv 

11 1 2

12 ru , ru  + 12 ru , rv  1 2 ru , rv  + 12 rv , rv 

12 1 2

22 ru , ru  + 22 ru , rv  1 2 ru , rv  + 22 rv , rv 

22

= ruu , rv  ,

(2.148)

= ruv , ru  ,

(2.149)

= ruv , rv  ,

(2.150)

= rvv , ru  ,

(2.151)

= rvv , rv  .

(2.152)

Equations (2.145) and (2.146) can be expressed in compact form as  T 1   u u  u =− , u T 2 u

(2.153)

where u = ( u v )T and the matrices 1 and 2 are symmetric.

2.3

Optimization Mathematical optimization seeks a solution, within some allowable set of values (search space), that maximizes or minimizes a function. We will restrict our attention to the minimization of an objective function where the allowable search space is represented by an equality condition.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.3 Optimization

2.3.1

29

Constrained Static Optimization The constrained static optimization problem seeks to find a solution, x, which minimizes some objective function, J (x), subject to the equality constraints f (x) = 0.

(2.154)

We define the manifold, P, as the level set of J (x), P  {x|J (x) = constant},

(2.155)

and the constraint manifold, C, as the zero set of f (x), C  {x| f (x) = 0}.

(2.156)

At a local extremum, xo, the tangent space of C lies in the tangent space of P, Txo (C) ⊆ Txo (P),

(2.157)

where Txo (C) = ker( f ∇xo )

and,

Txo (P) = ker(J ∇xo ).

(2.158)

Conversely, in terms of the normal spaces, Txo (P)⊥ ⊆ Txo (C)⊥ ,

(2.159)

Nxo (P) ⊆ Nxo (C).

(2.160)

Nxo (P) = ker(J ∇xo )⊥ = im(∇xo J )

(2.161)

Nxo (C) = ker( f ∇xo )⊥ = im(∇xo f ).

(2.162)

im(∇xo J ) ⊆ im(∇xo f ).

(2.163)

or

So, we have

and

Thus,

Defining the basis, NC , for the normal space of C, NC  {∇ f1 , · · · , ∇ fm },

(2.164)

we can represent ∇xo J in NC , ∇xo J = −

m 

λi ∇xo fi = −∇xo f λ,

(2.165)

i=1

where λ is a vector of Lagrange multipliers. The solution to the constrained minimization problem is then found by solving the following system: ∇J + ∇ f λ = 0 f (x) = 0.

(2.166)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

30

Mathematical Preliminaries

Example: Let us consider a quadratic objective function J (x) = xT Bx,

(2.167)

where B is a symmetric positive definite matrix. Let us also consider a set of constraints f (x) = Ax − y = 0.

(2.168)

J (x + dx) = (x + dx)T B(x + dx) = xT Bx + 2xT Bdx.

(2.169)

dJ = J (x + dx) − J (x) = 2xT Bdx = J ∇dx.

(2.170)

We note that

So,

Thus, J ∇ = 2xT B

and

∇J = 2Bx.

(2.171)

We also note that f∇ = A

and

∇f = AT .

(2.172)

So the extremum must satisfy ∇J + ∇ f λ = 2Bx + AT λ = 0,

(2.173)

f (x) = Ax − y = 0.

(2.174)

1 x = − B−1 AT λ. 2

(2.175)

and

Solving for x in 2.173 yields

Substituting into 2.174 and solving for λ yields λ = −2(AB−1 AT )−1 y,

(2.176)

x = B−1 AT (AB−1 AT )−1 y.

(2.177)

and our solution is

If the weighting matrix, B, is the identity matrix, then our solution is equivalent to the least norm solution from Section 2.1.1, x = AT (A−1 AT )−1 y = A† y.

(2.178)

Figure 2.6 provides a geometric illustration of the constrained static optimization problem.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.4 Exercises

31

level sets of

Figure 2.6 Minimization of J (x) = xT Bx subject to the constraint Ax = y. The solution corresponds to the point on the constraint surface (line) where the gradient of the objective function is orthogonal to the constraint tangent space.

2.4

Exercises 1. Project the vector, x = ( −3 1 4)T onto the basis

T u1 = 1 0 0 , T  u2 = 0 √12 √12 , T  u3 = 0 − √12 √12 . That is, find the coefficients, ci , associated with the linear combination of basis vectors where x = c1 u1 + c2 u2 + c3 u3 . 2. Construct an orthonormal basis, {u1 , u2 , u3 }, from

T v1 = 1 1 2 0 ,

T v 2 = 3 0 −2 1 ,

T v 3 = −2 1 1 −2 using the Gram-Schmidt process. 3. Find the line, y = mx + b, that best fits the points (x1 , y1 ) = (1, .5), (x2 , y2 ) = (−2, −3.8), (x3 , y3 ) = (−1, −3), and (x4 , y4 ) = (3, 2.8), in a least squares sense.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

32

Mathematical Preliminaries

4. Consider the system 

1 1 −2 1

⎞ x1   ⎟ 1 −1 ⎜ ⎜ x2 ⎟ = 3 . 3 0 ⎝ x3 ⎠ 4 x4 

⎛

(a) Find the least norm solution, xP . (b) Represent all solutions to the system by finding a basis for the null space of A. 5. Consider the matrix ⎞ ⎛5 5 1 3 4 ⎟ ⎜ A = ⎝ 54 1 56 ⎠ . 1

5 6

3 2

(a) Find the eigenvalues and eigenvectors. (b) Show that D = S−1 AS, where D is the diagonal matrix of eigenvalues and S is the matrix of eigenvectors. 6. Consider the time-parameterized curve (see Figure 2.7) given by

T r(t ) = 2 cos(t/2) −3 sin(t/2) cos(t ) . (a) Compute the velocity (tangent vector) of a point moving along the curve. (b) Compute the speed, s (t ), of a point moving along the curve.

Figure 2.7 (Left) Space curve r(t ) = (2 cos(t/2), −3 sin(t/2), cos(t )) (Exercise 6). (Right) Space

curve r(t ) = (cos(t ), sin(t ), t/5) (Exercise 7).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.4 Exercises

33

7. Consider the time-parameterized curve (see Figure 2.7) given by

T r(t ) = cos(t ) sin(t ) t/5 . (a) Compute the velocity (tangent vector) of a point moving along the curve. (b) Compute the arc length of the curve on the interval [0, 8π ]. (c) Compute the curvature of the curve. 8. Generate the Gauss map for the surface of Figure 2.8 with implicit description  2 1 2 2 f = 4 + x + y − z − cos(3x) cos(3y) = 0. 3



Figure 2.8 Implicit surface f = 4 + x2 + y2 − z −

1 3

cos(3x) cos(3y)

2

= 0 (Exercise 8).

9. Consider the parameterized surface (see Figure 2.9) given by

T r(u, v) = u v − 25 (u2 + v 2 ) + cos(2u) cos(2v) and the parameterized curve

T   α(t ) = t t − 34 − 25 t 2 + (t − 34 )2 + cos(2t ) cos(2t − 32 ) sitting on the surface. (a) Generate the Gauss map for the surface. (b) Determine the total (extrinsic) curvature, k, of α(t ) at point t = 3/8. (c) Determine the normal, kn , and geodesic curvature, kg , of α(t ) at the point t = 3/8.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

34

Mathematical Preliminaries





Figure 2.9 Parametric surface r(u, v) = u,v, − 25 (u2 + v2 ) + cos(2u) cos(2v) and 3 2 3 2 3 2

parameterized curve α(t ) = t, t − 4 , − 5 t + (t − 4 )

+ cos(2t ) cos(2t −

2

(Exercise 9).

Figure 2.10 Implicit surface f = z − xy = 0 (Exercise 10).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

2.4 Exercises

35

10. Consider the implicit surface (see Figure 2.10) given by f = z − xy = 0. (a) Generate the Gauss map for the surface. (b) Compute the curvature tensor, K . (c) Determine the principal curvatures, κ1 and κ2 , the Gaussian curvature, K, and the mean curvature, H , at the point (x, y) = (0, 0). 11. Show that the geodesics of a Euclidean plane are straight lines.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:30:07, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.004

3

Kinematics of Discrete Systems

Kinematics is the formal study of motion without regard to the causes of motion (i.e., forces and moments). It is a natural prerequisite to the study of dynamics, which builds on kinematics by addressing the kinetic causes of motion. The kinematics of discrete systems is concerned with systems that possess a finite number of degrees of freedom, as opposed to continuous systems which possess an infinite number of degrees of freedom. The basic element of a discrete system is the rigid body. In this chapter we will address spherical kinematics associated with pure rotation, followed by spatial kinematics which involves both rotation and translation. Finally, we will address the kinematics of systems of rigid bodies forming linked chains. This will provide the necessary formal understanding of the geometry of motion required to address dynamical principles in the subsequent chapters.

3.1

Spherical Kinematics Spherical kinematics is concerned with the 3-dimensional rotation group denoted as SO(3). This is the group of all rotations about the origin in Euclidean space, R3 . Formally, the group is defined as the set of all proper orthogonal matrices, Q. That is, SO(3) = {Q|Q ∈ R3×3 , QT Q = QQT = 1}.

(3.1)

The orthogonal rotation matrix, Q, will be described here, as well as some other ways to parameterize rotation. These include angle-sets, axis-angle parameters, and unit quaternions.

3.1.1

Orthogonal Rotation Matrices Orthogonal rotation matrices encode spatial rotation by describing the orientation of one coordinate frame relative to another. The column vectors of a rotation matrix are the basis vectors of the coordinate frame of interest, expressed within an embedding frame. For example, Figure 3.1 depicts frame B rotated relative to frame A. The rotation matrix describing the orientation of frame B in frame A is ⎞ ⎞ ⎛ eˆ · eˆ ⎛ eˆB2 · eˆA1 eˆB3 · eˆA1 B1 A1 ↑ ↑ ↑ ⎟ ⎜ A (3.2) Q = ⎝ AeˆB1 AeˆB2 AeˆB3 ⎠ = ⎝ eˆB1 · eˆA2 eˆB2 · eˆA2 eˆB3 · eˆA2 ⎠ . B ↓ ↓ ↓ eˆB · eˆA eˆB · eˆA eˆB · eˆA 1

3

2

3

3

3

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

37

3.1 Spherical Kinematics

Figure 3.1 Rotation of frame B relative to frame A. The basis vectors of frame B, expressed in

frame A, are the column vectors of the rotation matrix.

For rotations about the principal axes, ⎛ ⎞ 1 0 0 ⎜ ⎟ Qx (θ ) = ⎝ 0 cos θ − sin θ ⎠ , 0

sin θ

cos θ ⎛

cos θ

⎜ Qz (θ ) = ⎝ sin θ

⎛

cos θ

0

0

1

−sin θ ⎞

0

⎜ Qy (θ ) = ⎝ −sin θ cos θ

0

0

0

sin θ

⎞

⎟ 0 ⎠,

cos θ

(3.3)

⎟ 0⎠. 1

The inverse of a rotation matrix, Q−1 , satisfies ⎛

1

⎜ QQ−1 = Q−1 Q = 1 = ⎝ 0 0 Since Q is an orthogonal matrix, ⎛ ← AeˆB1 ⎜ A TA Q BQ = ⎝ ← AeˆB2 B ←

A

→

⎞⎛

→

0

⎞

1

⎟ 0⎠.

0

1

↑

⎟⎜ → ⎠ ⎝ AeˆB1

eˆB3

0

↑ A

↓

eˆB2 ↓

↑

A

(3.4)

⎞

⎟ eˆB3 ⎠ = 1.

(3.5)

↓

Therefore, QABQ = ABQ−1 ABQ = 1,

(3.6)

QT = Q−1 .

(3.7)

B A

and

Rotational transformation can be accommodated with rotation matrices using the product A

v = ABQBv.

(3.8)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

38

Kinematics of Discrete Systems

ˆ and spin Figure 3.2 Axis-angle representation describing an arbitrary rotation using spin axis, k, angle, θ , parameters.

Additionally, multiple rotations can be concatenated using multiplication: Q = ABQBCQ.

A C

(3.9)

This process is not commutative. That is, in general, QBCQ = BCQABQ.

A B

(3.10)

Thus, the order in which successive finite (noninfinitesimal) rotations are performed affects the resulting orientation.

3.1.2

Axis-Angle Representation We begin by noting the following: t h e o r e m 3.1 There exists a spin axis and angle for any arbitrary orientation in R3 . This is known as Euler’s rotation theorem. The axis-angle representation specifies a spin axis about which a coordinate frame is rotated by a specified angle. Figure 3.2 depicts an arbitrary rotation described by the ˆ and spin angle, θ . axis-angle parameters, spin axis, k, We wish to determine the rotation matrix, ABQ, associated with the rotation depicted in Figure 3.2, in terms of the axis-angle parameters. Let us start by defining orthonormal ˆ Let coordinate frame K be defined by vectors ıˆ and jˆ to be orthogonal to unit vector k.  ˆ the basis vectors ıˆ, jˆ, k. Let K be the coordinate frame that K is rotated into. Then, 

Q = Qk (θ ) = AKQKKQKB Q,

A B

where

⎞ ⎛ ↑ ix ⎜ A A Aˆ ⎟ ⎜ A Q = ⎝ ıˆ jˆ k ⎠ = ⎝ iy K ⎛

↑

↓

↑

↓

↓

iz

jx

(3.11)

kx

⎞

jy

⎟ ky ⎠ ,

jz

kz

(3.12)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

39

and Q = Qz (θ ),

(3.13)

Q = KAQ = AKQT .

(3.14)

Q = Qk (θ ) = AKQQz (θ )KAQ,

(3.15)

K K

and K B

So, A B

and ⎞⎛ ↑ cos θ ⎜ A A Aˆ ⎟ ⎜ Qk (θ ) = ⎝ ıˆ jˆ k ⎠ ⎝ sin θ ⎛

↑

↓

↑

↓

↓

0

− sin θ cos θ 0

⎞⎛ ← ⎟⎜ ⎜ 0⎠⎝← 1 ← 0

The components of ıˆ and jˆ drop out, so ⎛ kx kx (1 − cθ ) + cθ

Qk (θ ) = ⎝ kx ky (1 − cθ ) + kz sθ kx kz (1 − cθ ) − ky sθ

kx ky (1 − cθ ) − kz sθ

ıˆT

A

jˆT

A A

kˆ

T

→

⎞

⎟ →⎟ ⎠. →

kx kz (1 − cθ ) + ky sθ

⎞

ky kz (1 − cθ ) − kx sθ ⎠,

ky ky (1 − cθ ) + cθ ky kz (1 − cθ ) + kx sθ

(3.16)

(3.17)

kz kz (1 − cθ ) + cθ

where sθ and cθ have been used as shorthand for sin(θ ) and cos(θ ). Conversely, the axis-angle parameters, expressed in terms of Q, are   −1 Q11 + Q22 + Q33 − 1 , θ = cos 2 Q32 − Q23 , 2 sin θ Q13 − Q31 , ky = 2 sin θ Q21 − Q12 . kz = 2 sin θ

kx =

3.1.3

(3.18)

Euler Sets The Euler angle scheme specifies a sequence of relative frame rotations about the principal axes. For example, an xyz sequence specifies a rotation of α about the x axis of the base frame, A. Next a rotation of β is specified about the y axis of the intermediate frame associated with the completion of the first rotation, A . Finally a rotation of γ is specified about the z axis of the intermediate frame associated with the completion of the second rotation, A . Figure 3.3 depicts this sequence. The rotation matrix, ABQ, associated with this rotation sequence is given by 



Q(α, β, γ ) = AAQ(α)AAQ(β )AB Q(γ ).

A B

(3.19)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

40

Kinematics of Discrete Systems

Figure 3.3 An xyz Euler angle sequence specifies a rotation of α about the x axis of the base

frame, A. Next, a rotation of β is specified about the y axis of the intermediate frame, A . Finally a rotation of γ is specified about the z axis of the intermediate frame, A .

Since all of these intermittent rotations are about principal axes, we have Q(α, β, γ ) = Qxyz (α, β, γ ) = Qx (α)Qy (β )Qz (γ ) ⎛ ⎞⎛ ⎞⎛ 1 0 0 cos(β ) 0 sin(β ) cos(γ ) −sin(γ ) ⎜ ⎟⎜ ⎟⎜ = ⎝ 0 cos(α) − sin(α) ⎠ ⎝ 0 1 0 ⎠ ⎝ sin(γ ) cos(γ ) A B

0

sin(α)

cos(α)

−sin(β )

0 cos(β )

0

0

0

⎞

⎟ 0⎠. 1 (3.20)

So, ⎛

−cβsγ

cβcγ

⎜ Q(α, β, γ ) = ⎝ sαsβcγ + cαsγ

A B

−cαsβcγ + sαsγ

−sαsβsγ + cαcγ cαsβsγ + sαcγ

sβ

⎞

⎟ −sαcβ ⎠ .

(3.21)

cαcβ

In general, for an arbitrary sequence abc, where a = x, y, or z, b = x, y, or z, c = x, y, or z,

(3.22)

a = b, b = c, we have Qabc (α, β, γ ) = Qa (α)Qb (β )Qc (γ ).

(3.23)

The inverse problem of determining the Euler angles in terms of the rotation matrix requires a bit more effort. For the xyz sequence addressed earlier, we have the following solution.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

41

 If cos β = 0, (β = ±π 2),



 β = Atan2(Q13 , Q211 + Q212 ),

(3.24)

α = Atan2(−Q23 /cβ, Q33 /cβ ),

(3.25)

γ = Atan2(−Q12 /cβ, Q11 /cβ ).

(3.26)

If β = ±π 2, α=0

(3.27)

γ = Atan2(Q21 , Q22 ).

(3.28)

For an xzx Euler sequence we have ⎛ cβ −sβcγ ⎜ Qxzx (α, β, γ ) = ⎝ cαsβ cαcβcγ − sαsγ sαsβ

sαcβcγ + cαsγ

sβsγ

⎞

⎟ −cαcβsγ − sαcγ ⎠ ,

(3.29)

−sαcβsγ + cαcγ

and the solution to the inverse problem is If sin β = 0, (β = 0, π ),  β = Atan2( Q221 + Q231 , Q11 ),

(3.30)

α = Atan2(Q31 /sβ, Q21 /sβ ),

(3.31)

γ = Atan2(Q13 /sβ, −Q12 /sβ ).

(3.32)

If β = 0, α=0

(3.33)

γ = Atan2(Q32 , Q22 ).

(3.34)

If β = π ,

3.1.4

α=0

(3.35)

γ = Atan2(Q32 , −Q22 ).

(3.36)

Quaternions A quaternion is a hypercomplex number which is the 4-dimensional analog to the 2-dimensional complex number z = a + bi. The 4-dimensional space of quaternions, denoted by H, is spanned by four orthogonal axes. These include the real axis and three principal imaginaries, i, j, and k: h = h0 + h1 i + h2 j + h3 k.

(3.37)

This can also be thought of as a scalar part, h0 , and a vector part, h: h = h0 + h.

(3.38)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

42

Kinematics of Discrete Systems

Quaternion algebra is noncommutative with respect to multiplication. Specifically, the following laws apply: i2 = j 2 = k2 = i jk = −1, i j = k = − ji, jk = i = −k j,

(3.39)

ki = j = −ik. Multiplication of two quaternions can most easily be represented using the operations of vector cross-product and dot product: s = gh = s0 + s s = g0 h0 − g · h + g0 h + h0 g + g × h.

(3.40)

Grouping the scalar and vector parts separately, we have s0 = g0 h0 − g · h s = g0 h + h0 g + g × h.

(3.41)

It is stressed that quaternions are not vectors but rather hypercomplex numbers. Nevertheless, the components of the imaginary part of a quaternion can be used in traditional vector operations to compute quaternion products per the preceding formula. An equivalent algorithm exists for representing multiplication of two quaternions using matrix operations: 



(3.42) s0 s1 s2 s3 = g0 g1 g2 g3 H , where H is an antisymmetric matrix defined in terms of h as ⎛ ⎞ h0 h1 h2 h3 ⎜ ⎟ ⎜ −h1 h0 −h3 h2 ⎟ ⎜ ⎟. H =⎜ h3 h0 −h1 ⎟ ⎝ −h2 ⎠ −h3 −h2 h1 h0

(3.43)

It is also convenient to represent a quaternion using complex matrices. First we define the following matrices in C2×2 :         1 0 i 0 0 1 0 i 1= , I= , J= , K= . (3.44) 0 1 0 −i −1 0 i 0 These are derived from the so-called Pauli matrices. The principal imaginaries, I, J, and K, adhere to the same laws stated earlier, namely, I 2 = J 2 = K 2 = IJK = −1, IJ = K = −JI, JK = I = −KJ ,

(3.45)

KI = J = −IK.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

The quaternion is then



H = h0 1 + h1 I + h2 J + h3 K =

h0 + ih1

h2 + ih3

−h2 + ih3

h0 − ih1

43

 .

(3.46)

While this is a useful representation of quaternions, we will use the conventional representation throughout the remainder of this text, namely, h = h0 + h1 i + h2 j + h3 k. We can define the inverse of a quaternion, h−1 , as satisfying hh−1 = h−1 h = 1 + 0i + 0 j + 0k = 1.

(3.47)

¯ is given by Noting that the product of a quaternion and its conjugate, h, hh¯ = (h0 + h1 i + h2 j + h3 k)(h0 − h1 i − h2 j − h3 k) = h0 h0 − h · h + h0 h + h0 h + h × h = h0 h0 + h1 h1 + h2 h2 + h3 h3 = h2 , (3.48) we have h−1 =

h¯ . h2

(3.49)

A unit quaternion {h|h = 1} is a point on a unit hypersphere, S3 ⊂ H, where h20 + + h22 + h23 = 1. Unit quaternions are efficient at encoding spatial rotation. There is a direct relationship between the elements of a unit quaternion and the axis-angle parameters. For any unit quaternion, h ∈ S3 , we have h21

h = cos

θ θ θ θ θ θ + kx sin i + ky sin j + kz sin k = cos + u sin , 2 2 2 2 2 2

(3.50)

where u = kx i + ky j + kz k.

(3.51)

In shorthand, we have θ

h = e2u

and

θ

h−1 = h¯ = e− 2 u .

(3.52)

The individual elements of a unit quaternion are also referred to as Euler parameters, 1 , 2 , 3 , where 1  h1 , 2  h2 , 3  h3 , 4  h0 , θ θ θ θ 1 = kx sin , 2 = ky sin , 3 = kz sin , 1 = cos . 2 2 2 2

(3.53)

Rotational transformation can be accommodated with unit quaternions using a double product. To rotate a vector v ∈ R3 about the u axis by an angle of θ , we perform the following: θ

θ

hvh−1 = hv h¯ = e 2 u ve− 2 u .

(3.54)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

44

Kinematics of Discrete Systems

In this case the vector, v, is represented as a pure quaternion (real part equal to zero): v = v1 i + v2 j + v3 k.

(3.55)

In terms of frame transformations we have A

¯ v = ABhBv BAh = ABhBv ABh,

(3.56)

h¯ = BAh.

(3.57)

where A B

Additionally, multiple rotations can be concatenated using multiplication, for example, h = ABhBCh.

A C

(3.58)

As with rotation matrices, this process is not commutative. That is, in general, h h = BChABh.

A B B C

(3.59)

Thus, for both rotation matrices and unit quaternions, the order in which successive finite (noninfinitesimal) rotations are performed affects the resulting orientation. It is useful to relate the unit quaternion to the other representations of orientation. For axis-angle parameters we have θ = 2 cos−1 h0 ,

h0 = cos θ2 , h1 = kx sin θ2 , h2 = ky sin θ2 , h3 = kz sin θ2 ,

and

kx =

√ h1 , 1−h0 h0

ky =

√ h2 , 1−h0 h0 √ h3 . 1−h0 h0

kz =

(3.60)

Relating a rotation matrix and unit quaternion, we have ⎛ ⎞ 2(h0 h0 + h1 h1 ) − 1 2(h1 h2 − h0 h3 ) 2(h1 h3 + h0 h2 ) ⎜ ⎟ Q(h) = ⎜ 2(h0 h0 + h2 h2 ) − 1 2(h2 h3 − h0 h1 ) ⎟ ⎝ 2(h1 h2 + h0 h3 ) ⎠ (3.61) 2(h2 h3 + h0 h1 ) 2(h0 h0 + h3 h3 ) − 1 2(h1 h3 − h0 h2 ) and h0 =

1 1 + Q11 + Q22 + Q33 , 2 Q32 − Q23 , h1 = 4h0 Q13 − Q31 h2 = , 4h0 h3 =

(3.62)

Q21 − Q12 . 4h0

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

45

Figure 3.4 An infinitesimal rotation involving a differential spin, θ, about a fixed instantaneous

axis.

3.1.5

Calculus of Rotations It will be useful to examine the rate of change for some of the orientation schemes that we have described. In particular, we can express the differential change of rotation matrices and unit quaternions as a differential spin about a fixed instantaneous axis. In doing so, we arrive at the concept of angular velocity, thereby relating the derivative of an orientation operator to angular velocity. A relationship between angle-set derivatives and angular velocity can also be derived. We will now define the derivative of a rotation operator. Let us begin with the rotation matrix Q(t + t ) − Q(t ) dQ = lim . Q˙ = t→0 dt t

(3.63)

It will be convenient to express the time rate of change of a rotation operator in terms of an instantaneous spin rate and axis. This will entail applying a differential spin, θ , about a fixed instantaneous axis. Figure 3.4 depicts this infinitesimal rotation. Noting that Q(t + t ) = Qk (θ )Q(t ),

(3.64)

the derivative can then be expressed as Qk (θ )Q(t ) − Q(t ) = Q˙ = lim t→0 t



 Qk (θ ) − 1 lim Q(t ). t→0 t

(3.65)

Noting small angle (infinitesimal) properties, Qk (θ ) is ⎛

1

⎜ Qk (θ ) = ⎝ kz θ −ky θ

−kz θ 1 kx θ

ky θ

⎞

⎟ −kx θ ⎠ .

(3.66)

1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

46

Kinematics of Discrete Systems

We then have ⎛

0

⎜ Q˙ = ⎝ kz θ˙ −ky θ˙

−kz θ˙ 0 kx θ˙

⎛ ⎞ 0 ky θ˙ ⎜ ⎟ ˙ −kx θ ⎠ Q(t ) = ⎝ ωz

−ωz 0

−ωy

0

ωx

ωy

⎞

⎟ −ωx ⎠ Q(t ),

(3.67)

0

where ω = ( ωx ωy ωz )T is the angular velocity vector. Defining the antisymmetric angular velocity tensor, , as ⎛ ⎞ ωy 0 −ωz ⎜ ⎟  = ⎝ ωz (3.68) 0 −ωx ⎠ , −ωy we have

⎛

ωx

0

⎜ Q˙ = Q = ⎝ ωz −ωy

0

−ωz 0

ωy

⎞

⎟ −ωx ⎠ Q.

ωx

(3.69)

0

The matrix, , is a rank 2 tensor and can thus be represented as the following dyadic: =

3  3 

i j eˆi ⊗ eˆ j = ωz (eˆ2 ⊗ eˆ1 − eˆ1 ⊗ eˆ2 )

i=1 j=1

+ ωy (eˆ1 ⊗ eˆ3 − eˆ3 ⊗ eˆ1 ) + ωx (eˆ3 ⊗ eˆ2 − eˆ2 ⊗ eˆ3 ).

(3.70)

This has been formulated with the spin axis represented in the base frame. Any frame representation can be used: Q˙ = A ABQ = ABQB.

A B

(3.71)

Frame transformations for the angular velocity tensor can be accommodated with the tensor double product  = ABQB BAQ.

A

(3.72)

The product of the angular velocity tensor with a vector is equivalent to the crossproduct of the angular velocity vector and a vector, that is, A

A ArP = AωA × ArP .

(3.73)

Defining the derivative of a unit quaternion, we have h(t + t ) − h(t ) dh = lim . (3.74) h˙ = t→0 dt t Applying a differential spin, θ , about a fixed instantaneous axis (see Figure 3.4) gives us h(t + t ) = hk (θ )h(t ). The derivative can then be expressed as hk (θ )h(t ) − h(t ) = h˙ = lim t→0 t

 hk (θ ) − 1 h(t ), lim t→0 t

(3.75)



(3.76)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

47

Table 3.1 Dualities between the rotation matrix and the unit quaternion Property

Identity

Rotation Matrix ⎞ ⎛ 1 0 0 ⎟ ⎜ 1 = ⎝0 1 0⎠ 0 0 1

Inverse Derivative ⎛ Angular velocity

h−1 = h¯

Q˙ = Q

h˙ = 12 ωh

ωy

−ωz

0

⎜  = ⎝ ωz A

1 = 1 + 0i + 0 j + 0k

Q−1 = QT ⎞

⎟ −ωx ⎠

0

−ωy Transformation

Quaternion

ωx

ω = 0 + ωx i + ωy j + ωz k

0

 = ABQB BAQ

A

ω = ABhBω BAh

where, in the limit, hk (θ ) is hk (θ ) = 1 + kx

θ θ θ i + ky j + kz k. 2 2 2

We then have    ˙ ˙ ˙  ˙h = 0 + kx θ i + ky θ j + kz θ k h(t ) = ωx i + ωy j + ωz k h(t ), 2 2 2 2 2 2

(3.77)

(3.78)

where ω is the angular velocity represented as a quaternion: ω = ωx i + ωy j + ωz k.

(3.79)

1 h˙ = ωh. 2

(3.80)

So, we have

As in the case of the derivative of the rotation matrix, this has been formulated with the spin axis represented in the base frame. Any frame representation can be used: 1 1 h˙ = Aω ABh = ABhBω. 2 2

A B

(3.81)

Frame transformations for the angular velocity quaternion can be accommodated with the familiar tensor double product, A

ω = ABhBω BAh.

(3.82)

Table 3.1 lists various dualities between the rotation matrix and the unit quaternion. We have thus far related rotation matrices and quaternions to angular velocity. We can also relate rates of change of Euler angle sets to angular velocity. The rate vector of

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

48

Kinematics of Discrete Systems

a given angle set is ⎛ ⎞ α˙ ⎜ ˙⎟ ˙ φ = ⎝β ⎠.

(3.83)

γ˙ Noting the relationship between a rotation matrix and angular velocity, ⎞ ⎛ ωy 0 −ωz ⎟ ⎜ ˙ T,  = ⎝ ωz 0 −ωx ⎠ = QQ −ωy

ωx

0

we can determine the following: ⎞ ⎛ ⎞ ⎛ ˙ Q31 Q21 + Q˙ 32 Q22 + Q˙ 33 Q23 ωx ⎟ ⎜ ⎟ ⎜ ˙ ⎟ ˙ ˙ ω = ⎝ ωy ⎠ = ⎜ ⎝ Q11 Q31 + Q12 Q32 + Q13 Q33 ⎠ . ωz

(3.84)

(3.85)

Q˙ 21 Q11 + Q˙ 22 Q12 + Q˙ 23 Q13

We note that ∂Qi j ˙ Q˙ i j = φ. ∂φ

(3.86)

So angular velocity is related to the angle set rate vector by the following relationship: ⎛ ⎞ ⎛ ∂Q31 Q + ∂Q32 Q + ∂Q33 Q ⎞ 21 22 23 ωx ∂φ ∂φ ∂φ ⎟ ⎜ ⎟ ⎜ ∂Q ∂Q ∂Q 11 12 13 ⎟ ˙ ˙ (3.87) ω = ⎝ ωy ⎠ = ⎜ ⎝ ∂φ Q31 + ∂φ Q32 + ∂φ Q33 ⎠ φ = E(φ)φ, ∂Q21 22 23 ωz Q11 + ∂Q Q12 + ∂Q Q13 ∂φ ∂φ ∂φ where E(φ) is the Jacobian between angular velocity and angle set rates.

Example: We now address the motion of an atmospheric reentry body. The task of determining the orientation history of a reentry body requires a knowledge of the gyroscope data. Figure 3.5 depicts an incremental change in a body’s orientation in R3 . The increˆ can be related to the instantaneous angular velocity, mental spin, θ , about an axis, k, ω, of the body. We can use quaternions as an efficient means of representing orientation resulting from incremental motion. Since the gyroscopes provide angular velocity in the body’s local reference frame, we can approximate the orientation quaternion, OCh(t + t ), of the body based on the previous orientation, OCh(t ), and the incremental quaternion, hk (θ ), associated with ˆ that is fixed during the a finite but small incremental rotation, θ , about an axis, k, rotation. This can be expressed as h(t + t )  OCh(t )hk (θ ).

O C

(3.88)

Note that the spin axis of the incremental rotation is represented in the local body frame in (3.88) as opposed to the base frame since the gyroscopes measure angular velocity in

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

49

ˆ Figure 3.5 Instantaneous spin of a body about an axis. The incremental spin, θ, about an axis, k, can be related to the instantaneous angular velocity, ω, of the body. Quaternions provide a numerically efficient means of integrating angular velocity to yield finite rotations.

the local body frame. Due to small angle properties, 1 θ θ θ i + ky j + kz k = 1 + ωt, 2 2 2 2 where ω is the angular velocity quaternion, hk (θ )  1 + kx

ω = 0 + ωx i + ωy j + ωz k.

(3.89)

(3.90)

Thus, for numerical integration, we can use the following relationship:   ωy t ωz t ωx t O O i+ j+ k . h(t + t )  C h(t ) 1 + C 2 2 2

(3.91)

If we had implemented this integration using orthogonal rotation matrices, we would have had ⎛ ⎞ ωy 1 −ωz ⎜ ⎟ O Q(t + t )  OCQ(t ) ⎝ ωz (3.92) 1 −ωx ⎠ t. C −ωy

ωx

1

However, the orthogonality properties of OCQ(t ) would degrade with successive multiplications at finite precision, causing significant problems. In the case of the unit quaternion relationship of (3.91) the only concern would be that the length of the quaternion would deviate from unity with successive multiplications. This could be easily corrected by renormalizing the quaternion as

 O h(t ) 1 + 12 ωt C O

 . (3.93) h(t + t )  C Oh(t ) 1 + 1 ωt C

2

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

50

Kinematics of Discrete Systems

Figure 3.6 Quaternion time history derived from integration of the gyroscope data. The first two

quaternion components are displayed. Units have been intentionally omitted.

It is also useful to represent the orientation in terms of Euler angles. We can convert from quaternions to rotation matrices using (3.61). We can then convert from Q(h) to an xzx Euler set, {α, β, γ }, using the solution for xzx Euler angles in terms of the components of Q, given by (3.30) through (3.36). Figures 3.6 and 3.7 display plots of quaternion data calculated from gyroscope data. The data were calculated using the algorithm described here. The quaternion elements h2 and h3 are of special interest since they encode the ky and kz components of the spin

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

51

Figure 3.7 Quaternion time history derived from integration of the gyroscope data. The last two

quaternion components are displayed. Units have been intentionally omitted.

ˆ These components correspond to the lateral axes of the reentry body. While unit axis, k. quaternions possess computational efficacy, Euler angles can provide an easier means of mentally decomposing the orientation of a body from 2-dimensional plots. Figures 3.8 and 3.9 display plots of the xzx Euler angles associated with the reentry body orientation. Of these angles, β is of particular interest since it indicates the angular displacement between the reentry body longitudinal axis and the base coordinate frame x-axis. The β

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

52

Kinematics of Discrete Systems

Figure 3.8 Euler angle time history derived from integration of the gyroscope data. The β Euler angle is displayed. Units have been intentionally omitted.

angle can also be related to the coning angle of the reentry body (discussed in Section 3.2.5).

Addition of Angular Velocities We will now address the additive property of infinitesimal rotations. Consider a rotation varying over time, Q(t ), which is the identity matrix at some time t. We take it to change due to two sequential infinitesimal rotations, Q(t + t ) = Qk1 (θ1 )Qk2 (θ2 ).

(3.94)

The derivative can then be expressed as Qk1 (θ1 )Qk2 (θ2 ) − 1 . Q˙ = lim t→0 t Noting small angle (infinitesimal) properties, the product, Qk1 Qk2 , is ⎛

(3.95)

1 − (k1y k2y + k1z k2z )θ1 θ2

⎜ Qk1 Qk2 = ⎝ k1z θ1 + k2z θ2 + k1x k2y θ1 θ2 −k1y θ1 − k2y θ2 + k1x k2z θ1 θ2 −k1z θ1 − k2z θ2 + k1y k2x θ1 θ2

···

1 − (k1x k2x + k1z k2z )θ1 θ2 k1x θ1 + k2x θ2 + k1y k2z θ1 θ2 k1y θ1 + k2y θ2 + k1z k2x θ1 θ2

⎞

⎟ · · · −k1x θ1 − k2x θ2 + k1z k2y θ1 θ2 ⎠ . (3.96) 1 − (k1x k2x + k1y k2y )θ1 θ2

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.1 Spherical Kinematics

53

Figure 3.9 Euler angle time history derived from integration of the gyroscope data. The α and γ Euler angles are displayed. Units have been intentionally omitted.

Removing all terms involving second-order infinitesimals, θ1 θ2 , ⎛ Qk1 Qk2 = ⎝

1

−k1z θ1 − k2z θ2

k1z θ1 + k2z θ2

1

−k1y θ1 − k2y θ2

k1x θ1 + k2x θ2

k1y θ1 + k2y θ2

⎞

−k1x θ1 − k2x θ2 ⎠.

(3.97)

1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

54

Kinematics of Discrete Systems

This demonstrates that, unlike finite rotations, infinitesimal rotations commute since we can observe that Qk1 Qk2 = Qk2 Qk1 . Thus, the order in which successive infinitesimal rotations are preformed does not affect the resulting orientation. We then have ⎛

0

⎜ Q˙ = ⎝ k1z θ˙1 + k2z θ˙2 −k1y θ˙1 − k2y θ˙2

−k1z θ˙1 − k2z θ˙2 0

⎞ k1y θ˙1 + k2y θ˙2 ⎟ −k1x θ˙1 − k2x θ˙2 ⎠

k1x θ˙1 + k2x θ˙2 ⎛ 0 ⎜ = ⎝ ω1z + ω2z

0 −ω1z − ω2z

−ω1y − ω2y

0

ω1y + ω2y

0 ⎞

⎟ −ω1x − ω2x ⎠ ,

ω1x + ω2x

⎞

⎟ −ω1x − ω2x ⎠ . (3.98)

ω1x + ω2x

So, the resultant angular velocity tensor is ⎛ 0 −ω1z − ω2z ⎜  = ⎝ ω1z + ω2z 0 −ω1y − ω2y

ω1y + ω2y

(3.99)

0

and the resultant angular velocity vector is ⎞ ⎛ ω1x + ω2x ⎟ ⎜ ω = ⎝ ω1y + ω2y ⎠ = ω1 + ω2 .

(3.100)

ω1z + ω2z Equation (3.100) can be interpreted as the vector sum of a body’s angular velocity, ω1 , relative to a given reference frame and the angular velocity, ω2 , of a second body relative to the first body. This yields the total angular velocity of the second body relative to the reference frame.

3.2

Spatial Kinematics

3.2.1

Homogeneous Transform Matrices We generalize the notion of a finite displacement by considering the composition of a translation and a rotation. A position vector, BrP , represented in frame B can be represented in frame A by considering the translation and rotation of frame B with respect to frame A (see Figure 3.10). That is, A

rP = AdO−−A−−O→B + ABQBrP .

This can be represented in homogeneous coordinates as B  A  A A rP Q dO−−A−−O→B rP B = , 1 0 1 1

(3.101)

(3.102)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.2 Spatial Kinematics

55

Figure 3.10 A homogenous transform generalizes the notion of a finite displacement by

composing a translation and a rotation together in the same operator.

where we define the homogenous transform matrix, AB T , as   A A −−−−→ Q d O O B A B A T  . B 0 1

(3.103)

As with pure rotations, multiple spatial displacements can be accommodated using matrix multiplication: A C

T = AB T BC T .

(3.104)

This process is not commutative. That is, in general, A B

T BC T = BC T AB T .

(3.105)

Thus, the order in which successive finite (noninfinitesimal) displacements are preformed affects the resulting position and orientation.

3.2.2

Screws We define a screw, , as the set {s, s , θ , d}, where s, s are the Plücker vectors of the screw axis and θ , d are the displacement variables (see Figure 3.11). The moment vector, s , is given by s = d−OP−→o × s.

(3.106)

The homogenous transformation matrix associated with this screw is given by      1 d−OP−→o Qs (θ ) 0 1 ds 1 − d−OP−→o Ts (θ , d ) = 0 1 0 1 0 1 0 1   Qs (θ ) [1 − Qs (θ )]d−OP−→o + ds = , (3.107) 0 1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

56

Kinematics of Discrete Systems

Figure 3.11 A screw description consisting of the screw axis, s, the moment vector, s , and the displacement variables, θ and d.

where ⎛

sx sx (1 − cθ ) + cθ

sx sy (1 − cθ ) − sz sθ

⎜ Qs (θ ) = ⎜ ⎝ sx sy (1 − cθ ) + sz sθ

sy sy (1 − cθ ) + cθ

sx sz (1 − cθ ) − sy sθ

sy sz (1 − cθ ) + sx sθ

sx sz (1 − cθ ) + sy sθ

⎞

⎟ sy sz (1 − cθ ) − sx sθ ⎟ ⎠. sz sz (1 − cθ ) + cθ (3.108)

Figure 3.12 illustrates this transformation sequence. Given a screw axis in a coordinate frame, B, B

B

s and,

B s = d−O−B−→ Po × s,

(3.109)

we can represent it in coordinate frame, A, where s = ABQBs.

(3.110)

A A B A A B −−−→ −−−−→ s = Ad−O−A−→ Po × s = (B Q dOB Po + dOA OB ) × (B Q s),

(3.111)

A B A A B −−−−→ s = (ABQBd−O−B−→ Po ) × (B Q s) + dOA OB × (B Q s).

(3.112)

B A A B −−−−→ s = ABQ(d−O−B−→ Po × s) + dOA OB × (B Q s).

(3.113)

A

For the moment vector we note that A

or A

Simplifying, we have A

In terms of homogenous transformation matrices, Ts = AB T BTs BA T .

A

(3.114)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

57

3.2 Spatial Kinematics

Figure 3.12 Transformation sequence of a screw displacement. First, the origin of the frame is −→ to the screw axis, s. The frame is then rotated by θ about s followed by a translated by d−OP o −→ . translation of d along s. Finally, the frame is translated by −d−OP o

3.2.3

Motors We express the spatial velocity vector of a rigid body as  v=

ω vO

 ,

(3.115)

where ω is the angular velocity of the body and v O is the translational velocity of a point on the body coincident with the origin. This is equivalent to a motor. We can decompose this spatial vector into a line vector and a free vector: 

ω vO



 =

ω − − → dOPo × ω



 +

0 ω d˙ ω

 .

(3.116)

˙ we have In terms of the screw parameters, {s, s , θ˙, d}, 

ω vO

 = θ˙

    s 0 ˙ +d . s s

(3.117)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

58

Kinematics of Discrete Systems

Solving for s and θ˙ in terms of ω, we have ω and θ˙ = ω . ω

s=

(3.118)

To solve for d˙ in terms of ω and v O , we first note that ˙ ˙  + ds. v O = θs

(3.119)

˙ · s. s · v O = θ˙s · s + ds

(3.120)

ω · vO . d˙ = s · v O = ω

(3.121)

So,

Thus,

Alternately, the pitch, ρ, is ρ=

ω · vO d˙ ω · v O . = = ω·ω θ˙ ω2

(3.122)

To solve for s in terms of ω and v O , we note that ˙ × s, s × v O = θ˙s × s + ds

(3.123)

s × v O = θ˙s × s = θ˙s × (d−OP−→o × s) = θ˙d−OP−→o .

(3.124)

or

Thus, d−OP−→o =

s × vO . θ˙

(3.125)

So, s = d−OP−→o × s =

3.2.4

(ω × v O ) × ω (s × v O ) × s = . θ˙ ω3

(3.126)

Wrenches We express the spatial force vector acting on a rigid body as   f , f = ϕO

(3.127)

where f is the force acting on the body and ϕO is the moment about the origin. This is equivalent to a wrench. We can decompose this spatial vector into a line vector and a free vector:       0 f f = + . (3.128) τ  ff  d−OP−→o × f ϕO

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.2 Spatial Kinematics

In terms of the screw parameters, {s, s , f , τ }, we have       s 0 f = f + τ . s s ϕO

59

(3.129)

Solving for s and f in terms of f , we have s=

f f

and

f = f.

(3.130)

To solve for τ in terms of f and ϕO , we first note that ϕO = f s + τ s.

(3.131)

s · ϕO = f s · s + τ s · s.

(3.132)

So,

Thus, τ = s · ϕO =

f · ϕO . f

(3.133)

Alternately, the pitch, ρ, is ρ=

f · ϕO τ f · ϕO = . = 2 f f·f f

(3.134)

To solve for s in terms of f and ϕO , we note that s × ϕO = f s × s + τ s × s,

(3.135)

s × ϕO = f s × s = f s × (d−OP−→o × s) = f d−OP−→o .

(3.136)

or

Thus, −−→ = dOP o

s × ϕO . f

(3.137)

So, s = d−OP−→o × s =

3.2.5

( f × ϕO ) × f (s × ϕO ) × s  f 3 . = f .

(3.138)

Derivatives in Rotating Reference Frames In taking derivatives of vector quantities we must take care to note how the quantity of interest is expressed and relative to what reference frame the derivative is sought. Because the goal is to use kinematic quantities (e.g., translational and angler velocity and acceleration) to formulate dynamical equations of motion, we are usually interested in computing derivatives relative to an inertial reference frame. A derivative representing the time rate of change of some quantity can be computed in an inertial reference frame or a noninertial reference frame, that is, one rotating relative to an inertial reference frame.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

60

Kinematics of Discrete Systems

If all quantities are extrinsically expressed in the coordinates of an inertial reference frame, then differentiation is straightforward. Often, for convenience, vector quantities will be expressed in a local noninertial reference. As time elapses, the reference frame rotates with respect to the inertial frame. If we take an instantaneous quantity like translational or angular velocity that is expressed in a rotating reference frame, we need to consider how it is changing relative to the local reference frame it is expressed in, as well as how that local reference frame is itself changing. To demonstrate, we take an arbitrary vector quantity, Az, expressed in a particular rotating reference, A. The value of Az at any instant is measured in A. What we will call the simple derivative of the expression is denoted by Az˙ and represents the time derivative of Az relative to the frame it is expressed in, A. We are interested in taking the derivative of the quantity relative to an inertial reference frame, O, but expressing the coordinates in A. We denote this operator as A

d . dt O

(3.139)

We can apply this operator to the vector, Az, where, expressing in terms of a limit, A

d A (1 + AO Q)(Az + AA Az) − Az . z = lim t→0 dt O t

(3.140)

This expresses that the reference frame has rotated by AO Q relative to the inertial reference frame but is still expressed in A. Similarly, the vector, Az, has changed by AA Az relative to the rotating reference frame, A, and is also expressed in A. We simplify this expression: A A  A A d A A z O Q A + z = lim z . (3.141) t→0 dt O t t By definition, A

A d A A Az = Az˙ z = lim t→0 t dt A

(3.142)

is the simple derivative of Az, and A

So, A

A d A d A z= z+ dt O dt A

A d O Q = Q˙ = AA . Q = lim t→0 t dt O

A

 d Q Az = Az˙ + AA Az = Az˙ + AωA × Az. dt O

(3.143)

(3.144)

Applying this to the differentiation of angular velocity, we have A

αA:O =

A

d A ωA = Aω˙ A + AA AωA = Aω˙ A + AωA × AωA = Aω˙ A .    dt O

(3.145)

0

As a shorthand in our notation, if the frame that differentiation is relative to is omitted, we will take that to imply that it is relative to the inertial frame, for example, A αA = AαA:O .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

61

3.2 Spatial Kinematics

Applying differentiation of translational velocity, we have A

aPA:O =

A

d A v P = Av˙ PA + AA Av PA = Av˙ PA + AωA × Av PA . dt O A

(3.146)

Example: We return to the example of an atmospheric reentry body. The task of determining the motion history of the center of mass (to be formally defined in the next chapter) of a reentry body requires a knowledge of the body’s initial conditions and the gyroscope and accelerometer data. Figure 3.13 depicts a configuration where an accelerometer is mounted in each of three locations (frames 1, 2, and 3). The following relationships exist between the acceleration of the center of mass and the accelerations of the accelerometer frames C

aO1 = C1Q1aO1 = CaGC + CωC × (CωC × Cd1 ) + CαC × Cd1 ,

(3.147)

C

aO2 = C2Q2aO2 = CaGC + CωC × (CωC × Cd2 ) + CαC × Cd2 ,

(3.148)

C

aO3 = C3Q3aO3 = CaGC + CωC × (CωC × Cd3 ) + CαC × Cd3 .

(3.149)

These three vector equations yield nine scalar equations and nine unknowns. The nine unknowns include the three center of mass acceleration components, Cx¨GC , Cy¨GC , and Cz¨GC , as well as the six acceleration components of frames 1, 2, and 3, which lie in directions orthogonal to the accelerometers. The other three acceleration components of frames 1, 2, and 3, are known explicitly from the accelerometer values, a1 , a2 , and a3 . For a general formulation where the accelerometers are mounted arbitrarily we have a linear system, Ax = b, at a given instant of time where A is defined as ⎞ ⎛ 1 −C1Q 0 0 ⎟ ⎜ ⎟ ⎜1 0 0 −C2Q ⎟ (3.150) A⎜ ⎜1 C 0 0 −3Q ⎟ ⎠ ⎝ 0 diag(eˆ1 )N diag(eˆ2 )N diag(eˆ3 )N

Figure 3.13 Body with acceleration components known at three different locations.

Accelerometer data taken at three different locations and gyroscope data for the body can be used to determine the acceleration of the center of mass.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

62

Kinematics of Discrete Systems

and N is a logical matrix defined as  Ni j 

1 if accelerometer i measures in the j direction 0 if accelerometer i does not measures in the j direction.

(3.151)

The vectors x and b are defined as ⎛C

aGC

⎛

⎞

⎜1 ⎟ ⎜ aO1 ⎟ ⎟ x⎜ ⎜ 2a ⎟ O ⎝ 2⎠ 3 aO3

and

−CωC × (CωC × Cd1 ) − CαC × Cd1

⎞

⎜ C ⎟ ⎜ − ωC × (CωC × Cd2 ) − CαC × Cd2 ⎟ ⎜ ⎟, b⎜ C ⎟ C C C C ) ( − − ω × ω × d α × d C C 3 C 3 ⎝ ⎠ a

(3.152)

where a  ( a1 a2 a3 )T is the vector of accelerometer values. Because the accelerometers are typically aligned with the center of mass coordinate frame, the preceding system can be reduced to three scalar equations and three unknowns. For example, we will be dealing with a case where the accelerometer for frame 1 measures in the +x-axis of the center of mass frame, the accelerometer for frame 2 measures in the +y-axis, and the accelerometer for frame 3 measures in the −z-axis. So we have C

  x¨GC = a1 − CωC × (CωC × Cd1 ) + CαC × Cd1 · eˆ1 ,

(3.153)

C

  y¨GC = a2 − CωC × (CωC × Cd2 ) + CαC × Cd2 · eˆ2 ,

(3.154)

  z¨GC = −a3 − CωC × (CωC × Cd3 ) + CαC × Cd3 · eˆ3 .

(3.155)

C

Transforming into the initial world coordinate frame O, using quaternions, we have O

aGC = OChCaGC COh.

(3.156)

It is noted that in the preceding case which involves quaternion operations, OaGC is taken to be an acceleration quaternion. For numerical integration we can use the following trapezoidal relationships: O

v GC (t + t )  Ov GC (t ) +

 1 O aGC (t ) + OaGC (t + t ) t 2

(3.157)

rGC (t + t )  OrGC (t ) +

 1 O v GC (t ) + Ov GC (t + t ) t, 2

(3.158)

O

where we have converted the acceleration quaternion, OaGC , back into a standard vector form. Having computed the center of mass trajectory and body orientation history, we can calculate the coning angle of the reentry body over the time series. Figure 3.14

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.2 Spatial Kinematics

63

Figure 3.14 Center of mass position time history and orientation time history relative to a world

frame. The coning angle, ϕ, is shown as the angle between body axis, ux , and the trajectory tangent, utraj .

Figure 3.15 Center of mass acceleration x component derived from gyroscope and accelerometer data. Units have been intentionally omitted.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

64

Kinematics of Discrete Systems

Figure 3.16 Center of mass acceleration y and z components derived from gyroscope and accelerometer data. Units have been intentionally omitted.

depicts the combined orientation and position information describing the body’s motion in R3 . The coning angle, ϕ, is defined as the angle between the reentry body longitudinal axis (x-axis) and the tangent to the trajectory at a given instant in time. Using the results of Section 3.1.5, we know the α and β Euler angles of the reentry body orientation. The reentry body x-axis unit vector in the base coordinate frame is

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

65

3.2 Spatial Kinematics

Figure 3.17 Center of mass velocity and position x components. Numerical integration was performed on the acceleration data. Units have been intentionally omitted.

thus ⎛

cos β

⎞

⎜ ⎟ ux = ⎝ cos α sin β ⎠ .

(3.159)

sin α sin β

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

66

Kinematics of Discrete Systems

Figure 3.18 Center of mass velocity and position y component. The integrated signals were conditioned with a high-pass filter. Units have been intentionally omitted.

Alternately, this vector can be expressed as the first column of Q: ⎛ ⎜ ux = Qeˆ1 = ⎝

2(h0 h0 + h1 h1 ) − 1 2(h1 h2 + h0 h3 )

⎞ ⎟ ⎠.

(3.160)

2(h1 h3 − h0 h2 )

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

67

3.2 Spatial Kinematics

Figure 3.19 Center of mass velocity and position z component. The integrated signals were conditioned with a high-pass filter. Units have been intentionally omitted.

The tangent to the trajectory is rGC . utraj = rG C

(3.161)

cos ϕ = ux · utraj .

(3.162)

So,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

68

Kinematics of Discrete Systems

Figure 3.20 Reentry body trajectory generated after final conditioning of the integrated signals.

The scale in the x direction is greatly compressed. Units have been intentionally omitted.

Figure 3.21 Coning angle of the reentry body determined from the quaternion orientation history

and the trajectory tangent. Units have been intentionally omitted.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

69

3.3 Kinematic Chains

Figures 3.15 and 3.16 display plots of the center of mass acceleration components derived from telemetry data. Figure 3.17 displays plots of the center of mass velocity and position in the x direction. Figures 3.18 and 3.19 display plots of the center of mass velocity and position in the y and z directions. The signals were conditioned with a highpass filter. After final conditioning of the integrated signals, a trajectory was generated. Figure 3.20 displays the center of mass trajectory of the reentry body. The scale in the x direction is greatly compressed. Figure 3.21 displays a plot of the coning angle.

3.3

Kinematic Chains

3.3.1

Denavit-Hartenberg Parameterization The Denavit-Hartenberg parameters provide a systematic convention for assigning reference frames to the links of kinematic chains, or so-called lower-pair mechanisms (Denavit and Hartenberg 1955). Figure 3.22 depicts the Denavit-Hartenberg frame transformations. The four parameters are referred to as the link length, ai , link twist, αi , link offset, di , and joint angle, θi . The transformation matrix, T , from link i − 1 to link i is given by i−1 i

T = Qx (αi−1 )Dx (ai−1 )Qz (θi )Dz (di ).

(3.163)

Figure 3.22 The Denavit-Hartenberg parameters provide a systematic convention for assigning

reference frames to the links of lower-pair mechanisms. The four parameters, shown here, are the link length, ai , link twist, αi , link offset, di , and joint angle, θi .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

70

Kinematics of Discrete Systems

So,

⎛ i−1 i

−sθi

0

cθi cαi−1

−sαi−1

cθi sαi−1

cαi−1

0

0

cθi

⎜ ⎜ sθi cαi−1 T =⎜ ⎜ sθ sα ⎝ i i−1 0

ai−1

⎞

⎟ −sαi−1 di ⎟ ⎟. cαi−1 di ⎟ ⎠ 1

(3.164)

The overall transformation matrix, from base link 0 to link n, is given by n 0 n

T = T T ··· 0 1

1 2

n−1 n

T =

i−1 i

T.

(3.165)

i=1

3.3.2

The Jacobian Matrix In this section we will define the kinematic Jacobians that describe the mapping from configuration space to Cartesian space. Configuration space is represented by a set of generalized coordinates, {qi , . . . , qn }, corresponding to the joint variables in our kinematic chain. That is, qi = θi , if joint i is revoute, and

qi = di , if joint i is prismatic.

(3.166)

We first note that displacements, relative to a fixed reference frame, are δr =

∂r δq ∂q

and

δθ =

∂θ δq. ∂q

(3.167)

∂v δq ∂ q˙

and

δω =

∂ω δq. ∂ q˙

(3.168)

Alternately, we could write δr = We will define 

∂r ∂v = ∂q ∂ q˙



and

∂θ ∂ω = . ∂q ∂ q˙

(3.169)

Consequently, the velocities, relative to a fixed reference frame, are v = q˙

ω = q, ˙

and

(3.170)

and the accelerations, relative to a fixed reference frame, are a = q¨ + (˙ + )q˙

and

˙ q, α = q¨ +  ˙

(3.171)

where  is the angular velocity tensor of the frame in which  is expressed, relative to a fixed frame. More generally, the Jacobian, J, is a linear map between a configuration space velocity vector and a task space velocity vector. The concept of task space will be discussed in Chapter 8. For our purposes here it will suffice to interpret a task vector as a spatial vector representing both position and orientation. Given a task vector, x(q), the mapping,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.3 Kinematic Chains

71

{J|q˙ → x}, ˙ between configuration space and task space is given by δx = Jδq

or

x˙ = J q. ˙

(3.172)

The Jacobian is given by J=

∂x ∂ x˙ = . ∂q ∂ q˙

If we take x to be the standard Cartesian vector in R6 , then     δr v δx = or x˙ = . δθ ω The Jacobian is then

 J=

  . 

(3.173)

(3.174)

(3.175)

The task acceleration is given by ˜ )q, x¨ = J q¨ + (J˙ + J ˙

(3.176)

˜ = diag(, ). 

(3.177)

where

The term  is the angular velocity tensor of the frame in which the Jacobian is expressed. We note       ˜ q˙ = q˙ = q˙ = q˙ . J (3.178) q˙ ω×ω 0 Furthermore, if the Jacobian is expressed in a fixed frame, such as the base frame, then  = 0 and x¨ = J q¨ + J˙q. ˙

(3.179)

If we take the task force vector, f , to be the standard Cartesian vector in R6 , then   f f = . (3.180) ϕ By the Principle of Virtual Work for statics (the general principle for dynamics will be covered in Chapter 5), τ · δq = f · δx ∀δq, δx.

(3.181)

So, τ T δq = f T Jδq ∀δq,

(3.182)

which implies τ T = f T J,

(3.183)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

72

Kinematics of Discrete Systems

or τ = JT f .

(3.184)

Relationship between the Jacobian and Screws We can alternately interpret the Jacobian in terms of screws. First, we recall that the spatial velocity vector is given by       ω s 0 v= = θ˙  + d˙ . (3.185) vO s s For a revolute joint (d˙ = 0) this reduces to v = θ˙ and for a prismatic joint (θ˙ = 0) we have v = d˙ Defining s as

  s s

(3.186)

  0 . s

(3.187)

  s s=  s

(3.188)

  0 s

(3.189)

for a revolute joint and as s=

for a prismatic joint, we can express the spatial velocity of link n as ⎞⎛ ⎞ ⎛ q˙1 ↑ ↑ ↑ n  ⎟⎜ . ⎟ ⎜ s · · · sn ⎟ ⎜ .. ⎟ , vn = q˙i si = ⎜ ⎠⎝ ⎠ ⎝ 1 i=1 ↓ ↓ ↓ q˙

(3.190)

n

where the columns of the Jacobian,

⎛

↑

↑

⎜ J = ⎝ s1

···

↓

↓

↑

⎞

⎟ sn ⎠ ,

(3.191)

↓

are the screw vectors, s, in the appropriate coordinate frame.

3.3.3

Propagation of Kinematic Rate Vectors Two Rigid Bodies Connected by a Revolute Joint We have seen that angular velocities serially propagate in an additive fashion. Given two links, i − 1 and i, connected by a revolute joint with joint angle, θi , the angular velocity

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.3 Kinematic Chains

73

of link i is i

ωi = iωi−1 + θ˙i izˆi = ii−1Qi−1ωi−1 + θ˙i izˆi ,

(3.192)

where joint i is taken to rotate about the local zˆ axis. Equation (3.192) simply represents the vector sum of the angular velocity of link i − 1 and the angular velocity of link i, relative link i − 1, to yield the total angular velocity of link i. The angular acceleration of link i is given by the inertial differentiation of iωi , i i

αi =

i i d i d i d ˙i ωi = ωi−1 + (θi zˆi ). dt O dt dt O O    iα

(3.193)

i−1

Furthermore, the angular acceleration of link i − 1 is given by the inertial differentiation of i−1ωi−1 , i−1

d i−1 ωi−1 = i−1ω˙ i−1 + i−1i−1 i−1ωi−1 = i−1ω˙ i−1 . dt O Transforming the frame that i−1αi−1 is expressed in from i − 1 to i, we have αi−1 =

i−1

i

αi−1 = ii−1Qi−1αi−1 = ii−1Qi−1ω˙ i−1 .

(3.194)

(3.195)

The last term in (3.193) is given by

i  i d ˙i d ˙i d (θi zˆi ) = (θi zˆi ) + Q θ˙i izˆi dt O dt i dt O i i i ˙ ¨ = θi zˆi + i θi zˆi = θ¨i izˆi + iωi × θ˙i izˆi .

(3.196)

ωi × θ˙i izˆi = (iωi−1 + θ˙i izˆi ) × θ˙i izˆi = iωi−1 × θ˙i izˆi .

(3.197)

i

We further note that i

Finally, by substituting expressions from (3.195), (3.196), and (3.197) into (3.193), we have the desired expression for iαi : i

αi = iαi−1 + θ¨i izˆi + iωi−1 × θ˙i izˆi = ii−1Qi−1αi−1 + θ¨i izˆi + ii−1Qi−1ωi−1 × θ˙i izˆi .

(3.198)

The translational velocity of the proximal end of link i can be expressed as the vector sum of the translational velocity of the proximal end of link i − 1 and the translational velocity induced by the angular velocity of link i − 1: i

−−→ v i = iv i−1 + iωi−1 × id−(i−1)i −−→ ), = ii−1Q(i−1v i−1 + i−1ωi−1 × i−1d−(i−1)i

(3.199)

−−→ is the displacement vector from the proximal end of link i − 1 to the proxiwhere id−(i−1)i mal end of link i. The translational acceleration of the proximal end of link i is given by the inertial differentiation of iv i , i i

ai =

i i d i d i d i −−→ ). vi = v i−1 + ( ωi−1 × id−(i−1)i dt O dt dt O O   

(3.200)

ia i−1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

74

Kinematics of Discrete Systems

Furthermore, i

i d i d i −−→ ) = −−→ ) + ( ωi−1 × id−(i−1)i ( ωi−1 × id−(i−1)i dt O dt i

i

 d −−→ ) Q (iωi−1 × id−(i−1)i dt O

i i i −−→ +  −−−→ = iαi−1 × id−(i−1)i i−1 ( ωi−1 × d(i−1)i ) i i i −−→ + ω −−−→ = iαi−1 × id−(i−1)i i−1 × ( ωi−1 × d(i−1)i ).

(3.201)

−−→ and iω This represents the tangential and centripetal acceleration terms, iαi−1 × id−(i−1)i i−1 × i i −−−→ ( ωi−1 × d(i−1)i ), respectively. Finally, substituting (3.201) into (3.200), we have i

i i i −−→ + ω −−−→ ai = iai−1 + iαi−1 × id−(i−1)i i−1 × ( ωi−1 × d(i−1)i ) i−1 −−→ + −−→ )]. ωi−1 × (i−1ωi−1 × i−1d−(i−1)i = ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i

(3.202)

The translational velocity of the center of mass of link i can be expressed as v Gi = iv i + iωi × id−iG→i .

i

(3.203)

The translational acceleration of the center of mass of link i can be expressed as i

aGi = iai + iαi × id−iG→i + iωi × (iωi × id−iG→i ).

(3.204)

Two Rigid Bodies Connected by a Prismatic Joint Given two links, i − 1 and i, connected by a prismatic joint with joint displacement, di , the angular velocity of link i is simply i

ωi = iωi−1 = ii−1Qi−1ωi−1 .

(3.205)

The angular acceleration of link i is given by the inertial differentiation, i i

αi =

i d i d i ωi = ωi−1 . dt O dt  O  iα

(3.206)

i−1

Furthermore, the angular acceleration of link i − 1 is given by the inertial differentiation of i−1ωi−1 , i−1

αi−1 =

i−1

d i−1 ωi−1 = i−1ω˙ i−1 . dt O

(3.207)

The desired expression for iαi is then i

αi = iαi−1 = ii−1Qi−1αi−1 .

(3.208)

We note that since the prismatic joint produces no relative angular velocity, there is no propagation of angular velocity and acceleration. So, clearly, ωi = ωi−1 and αi = αi−1 . The translational velocity of the proximal end of link i is expressed as the vector sum of the translational velocity of the proximal end of link i − 1, the translational velocity induced by the angular velocity of link i − 1, and the translational displacement rate of

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

75

3.3 Kinematic Chains

link i, i

i −−→ + d˙ z v i = iv i−1 + iωi−1 × id−(i−1)i i ˆi i −−→ ) + d˙ z = ii−1Q(i−1v i−1 + i−1ωi−1 × i−1d−(i−1)i i ˆi ,

(3.209)

where joint i is taken to translate along the local zˆ axis. The translational acceleration of the proximal end of link i is given by the inertial differentiation of iv i , i i

ai =

i i i d i d i d i d ˙i −−→ ) + vi = v i−1 + ( ωi−1 × id−(i−1)i (di zˆi ). dt O dt dt dt O O O   

(3.210)

ia i−1

Furthermore, i

 d −−→ ) Q (iωi−1 × id−(i−1)i dt O  i d i i −−→ + ω −−−→ −−→ ) + ii−1 (iωi−1 × id−(i−1)i × d = iαi−1 × id−(i−1)i i−1 dt i (i−1)i

i d i d i −−→ ) = −−→ ) + ( ωi−1 × id−(i−1)i ( ωi−1 × id−(i−1)i dt O dt i

i

i ˙ i ˆi + iωi−1 × (iωi−1 × id−(i−1)i −−→ + ω −−→ ), = iαi−1 × id−(i−1)i i−1 × di z

(3.211)

and, i

i d ˙i d ˙i (di zˆi ) = (di zˆi ) + dt O dt i

i

 d Q d˙i izˆi = d¨i izˆi + ii−1 d˙i izˆi dt O

= d¨i izˆi + iωi−1 × d˙i izˆi .

(3.212)

Finally, substituting (3.211) and (3.212) into (3.210), we have i

i i i i ˙ i ˆi + d¨i izˆi −−→ + ω −−−→ ai = iai−1 + iαi−1 × id−(i−1)i i−1 × ( ωi−1 × d(i−1)i ) + 2 ωi−1 × di z i−1 −−→ + −−→ )] = ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i ωi−1 × (i−1ωi−1 × i−1d−(i−1)i

+ 2ii−1Qi−1ωi−1 × d˙i izˆi + d¨i izˆi .

(3.213)

In addition to the tangential and centripetal acceleration terms, we note the Coriolis acceleration term, 2iωi−1 × d˙i izˆi , and the joint acceleration term, d¨i izˆi , in (3.213). The translational velocity of the center of mass of link i can be expressed as i

v Gi = iv i + iωi × id−iG→i .

(3.214)

The translational acceleration of the center of mass of link i can be expressed as i

aGi = iai + iαi × id−iG→i + iωi × (iωi × id−iG→i ).

(3.215)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

76

Kinematics of Discrete Systems

Summary of Kinematic Propagation Equations For two links, i − 1 and i, connected by a revolute joint we have ωi = ii−1Qi−1ωi−1 + θ˙i izˆi ,

(3.216)

−−→ ), v i = ii−1Q(i−1v i−1 + i−1ωi−1 × i−1d−(i−1)i

(3.217)

i

i

i

v Gi = iv i + iωi × id−iG→i , i

αi = ii−1Qi−1αi−1 + θ¨i izˆi + ii−1Qi−1ωi−1 × θ˙i izˆi ,

(3.219)

i−1 −−→ + −−→ )], ai = ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i ωi−1 × (i−1ωi−1 × i−1d−(i−1)i

(3.220)

i

i

(3.218)

aGi = iai + iαi × id−iG→i + iωi × (iωi × id−iG→i ).

(3.221)

For two links, i − 1 and i, connected by a prismatic joint we have i

ωi = ii−1Qi−1ωi−1 ,

(3.222)

i −−→ ) + d˙ z v i = ii−1Q(i−1v i−1 + i−1ωi−1 × i−1d−(i−1)i i ˆi ,

(3.223)

i

i

−→ , v Gi = iv i + iωi × idiG i i

(3.224)

αi = ii−1Qi−1αi−1 ,

i

(3.225)

i−1 −−→ + −−→ )] ai = ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i ωi−1 × (i−1ωi−1 × i−1d−(i−1)i

+ 2ii−1Qi−1ωi−1 × d˙i izˆi + d¨i izˆi , i

3.4

i i i −→ + ω × ( ω × d−→ ). aGi = iai + iαi × idiG i i iGi i

(3.226) (3.227)

Kinematic Constraints and Degrees of Freedom The notions of constraints and degrees of freedom are important concepts that need to be formally described. The degrees of freedom of a kinematic system refer to the number of independent coordinates that are needed to fully describe the motion of the system. Constraints, on the other hand, act as restrictions to the motion of a kinematic system. For now we will consider one type of kinematic constraint referred to as a holonomic constraint. Holonomic constraints can be expressed as algebraic functions of the system’s generalized coordinates, exclusively. In Section 3.3 we referred to configuration space as the space of generalized coordinates corresponding to the joint variables in a kinematic chain. We can denote the number of generalized coordinates used to describe a kinematic chain as n. If the number of independent constraint equations that impose restrictions on those generalized coordinates is denoted by m, then the number of degrees of freedom, p, is given by p = n − m.

(3.228)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.5 Exercises

77

Figure 3.23 Branching and closed chain systems. (Left) Branching kinematic chains can be

expressed with a minimal set of generalized coordinates, corresponding to the joint variables, and no imposed constraints. (Right) Closed kinematic chains consisting of loop closures can be represented as branching chains with imposed holonomic constraints to enforce the loop closures.

The way a kinematic system is parameterized is not unique. For example, a rigid body connected to ground by a revolute joint could be parameterized by 6 generalized coordinates (n = 6) with 5 imposed constraints (m = 5). Such a system would have 1 degree of freedom (p = 6 − 5 = 1). Alternately, this system could be parameterized as a kinematic chain with a single generalized coordinate (n = 1) associated with the revolute joint and no imposed constraints (m = 0). With this parameterization the system would still have 1 degree of freedom (p = 1 − 0 = 1). In this way the number of degrees of freedom of a kinematic system is invariant of a particular parameterization. Branching kinematic chains like the ones addressed in Section 3.3 can be expressed with a minimal set of generalized coordinates, corresponding to the joint variables, and no imposed constraints. Therefore, for these branching chains parametrized with this minimal set of joint coordinates, the number of generalized coordinates is equivalent to the number of degrees of freedom (p = n). Closed kinematic chains consisting of loop closures represent common holonomic constraints. Such systems can be represented as branching chains with imposed constraints to enforce the loop closures (see Figure 3.23). In the figure on the right, the closed chain is modeled by two open chains that meet at their terminal points (black sphere). The constraint equations, φ(q) = 0, describe the enforcement of the loop closure. In the subsequent chapter we shall see how the constraints are incorporated into the dynamical equations.

3.5

Exercises 1. Given the basis representation for each frame shown in Figure 3.24, determine the rotation matrix, ABQ.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

78

Kinematics of Discrete Systems

Figure 3.24 Rotation of frame B relative to frame A. A basis for each frame is shown (Exercise 1).

2. Consider the rotation matrix ⎛

7 12

⎜  √  ⎜ 1 Q = ⎜ 12 −2 + 3 2 ⎝ 1 + √12 12

1 12

 √  −2 − 3 2 5 6

1 12

 √  −2 + 3 2

1 12



1 12

−

√1 2

⎞

√ ⎟ ⎟ −2 − 3 2 ⎟ . ⎠ 7 12

(a) Compute the invariant axis (spin axis) by solving the eigenvalue problem for Q. What eigenvector corresponds to the spin axis? What is the eigenvalue associated with this eigenvector? (b) Show that the norm of all the eigenvalues is 1. Verify that the spin axis found in (a) agrees with the solution found by using (3.18). 3. Consider the zxz Euler sequence. (a) Compute the corresponding rotation matrix, Qzxz (α, β, γ ). That is, compute the rotation sequence, Qz (α)Qx (β )Qz (γ ). (b) Derive the inverse relationship. That is, relate the Euler angles α , β , and γ to the elements of the rotation matrix computed in (a). 4. Consider the yxz Euler sequence with angles α = π /3,

β = π /4, and γ = π /6.

(a) Compute the corresponding unit quaternion, hyxz (α, β, γ ). That is, compute the rotation sequence hy (α)hx (β )hz (γ ). (b) Find the spin axis and angle associated with the quaternion from (a).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.5 Exercises

79

5. What mathematical property do infinitesimal rotations possess that finite rotations do not? 6. Consider the rotation matrix Q(α(t ), β(t ), γ (t )) = Qx (α(t ))Qy (β(t ))Qz (γ (t )). (a) Compute the angular velocity tensor in the local frame by differentiating the rotation matrix. (b) Express the angular velocity vector associated with the angular velocity tensor computed in (a). (c) Represent the angular velocity from (b) as an angular velocity quaternion. (d) Express the Jacobian matrix, E(α, β, γ ), between the angular velocity and the rates of change of α , β , and γ . 7. Consider the rotation matrix from Exercise 6. (a) Compute the angular velocity tensor in the base frame by differentiating the rotation matrix. (b) Compute the angular velocity tensor in the base frame using tensor transformation on the angular velocity tensor computed in Exercise 6(a). 8. Consider the screw displacement, , where ⎛

1

⎞

1 ⎜ ⎟ s = √ ⎝ −1 ⎠ , 6 2

−−→ dOP o

⎛ ⎞ 1 ⎜ ⎟ = ⎝1⎠, 0

θ = π /3, and d = 1.

(a) Compute the moment vector s . (b) Compute the rotation matrix, Qs (θ ), associated with . (c) Compute the transformation matrix, T s (θ , d ), associated with . T 9. Compute the spatial velocity vector v = ωT v TO , given the motor parameters ⎛

3

⎛

⎞

1 ⎜ ⎟ s = √ ⎝ 1 ⎠, 11 −1

−2

⎞

⎜ ⎟ −−→ = ⎝ 3 ⎠ , dOP o 0

θ˙ = π /10, and d˙ = 1/4.

10. Given the frame representations of the RRR mechanism described in Figure 3.25, and the Denavit-Hartenberg parameters shown in Table 3.2, compute the kinematic transform, 03 T at the joint variable values of ( θ1 θ2 θ3 )T = ( π /12 π /8 −π /6 )T . Table 3.2 Denavit-Hartenberg parameters for RRR mechanism i

αi−1

ai−1

θi

di

1 2 3

0 π /8 −π /4

0 3 5/2

θ1 θ2 θ3

0 3/2 1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

80

Kinematics of Discrete Systems

Figure 3.25 Frame representations of a RRR joint mechanism. The Denavit-Hartenberg

parameters are shown (Exercises 10 and 12).

11. Given the frame representations of the PRR mechanism described in Figure 3.26, and the Denavit-Hartenberg parameters shown in Table 3.3, compute the kinematic transform, 03 T , at the joint variable values of ( d1 θ2 θ3 )T = ( 5/4 −π /8 −π /12 )T . Table 3.3 Denavit-Hartenberg parameters for PRR mechanism i

αi−1

ai−1

θi

di

1 2 3

0 −π /2 π /2

0 2 3

0 θ2 θ3

d1 1/2 1

12. Consider the mechanism described in Figure 3.25 and Table 3.2, at the joint variable values of (θ1 θ2 θ3 )T = (π /12 π /8 − π /6)T . (a) Compute the position vector of the end of link 2 in the base frame. That is, compute 0r23 (the point on link 2 to which link 3 attaches, expressed in frame 0). (b) Compute the Jacobian, 03 = ∂ 0r23 /∂q, of the position vector from (a). (c) Compute the angular velocity vector of link 2 in the base frame. That is, compute 0ω2 .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

3.5 Exercises

81

Figure 3.26 Frame representations of a PRR joint mechanism. The Denavit-Hartenberg

parameters are shown (Exercises 11 and 13).

i. Compute 0ω2 by taking the derivative of 02Q. ii. Compute 0ω2 by forward propagation of the angular velocities. ˙ associated with the (d) Compute the angular velocity Jacobian, 02 = ∂ 0ω2 /∂ q, angular velocity from (c). 13. Repeat Exercise 12 for the mechanism described in Figure 3.26 and Table 3.3, at the joint variable values of (d1 θ2 θ3 )T = (5/4 − π /8 − π /12)T . 14. Consider the serial chain robot of Figure 3.27. The joint axes are in the z directions of the local frames. Frame 1 is rotated by π /2 in the x direction of the base frame followed by a z-axis rotation of θ1 . Frame 2 is rotated by −π /2 in the x direction of frame 1 followed by a z-axis rotation of θ2 . The link lengths are l1 and l2 . (a) Compute the rotation matrices 01Q, 12Q, and 02Q. (b) Compute the terms iωi , iαi , iai , and iaGi for i = 1, 2 using forward kinematic propagation. 15. Determine the number of degrees of freedom for the following cases. Specify different ways of defining the generalized coordinates and imposed constraints. (a) Two rigid bodies connected to each other by a revolute joint but otherwise free to move. (b) Two rigid bodies connected to each other by a spherical (ball and socket) joint but otherwise free to move. (c) Three rigid bodies connected in series by two revolute joints but otherwise free to move.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

82

Kinematics of Discrete Systems

Figure 3.27 A 2 degree-of-freedom serial chain robot with joint angles θ1 and θ2 (Exercise 14). The link lengths are l1 and l2 . The centers of mass are at the geometric centers of the links. The joint axes are in the z directions of the local frames. Frame 1 is rotated by π /2 in the x direction of the base frame followed by a z-axis rotation of θ1 . Frame 2 is rotated by −π /2 in the x direction of frame 1 followed by a z-axis rotation of θ2 .

(d) Three rigid bodies connected in series by one revolute joint and one spherical joint but otherwise free to move. 16. Consider a system of two particles moving in R3 . Let the positions of the particles be represented as Or1 = ( q1 q2 q3 )T and Or2 = ( q4 q5 q6 )T . (a) How many degrees of freedom does the system possess? How many generalized coordinates are required to parameterize the system? (b) Let us consider that three linear constraints are imposed on the system ⎞ ⎛ 0 −c3 c2 ⎛ q1 ⎞ ⎟ ⎜ φ(q) = ⎝ c3 0 −c1 ⎠ ⎝ q2 ⎠ = 0. q3 −c2 c1 0 i. What type of constraints are these? Describe the constraints physically in terms of the behavior of the two particles. ii. How many degrees of freedom does the system have with these constraints imposed (be careful to consider the linear independence of the constraints)? (c) Now let us consider the following constraint imposed on the system: ⎛ ⎞ q4 − q1 ⎝ ⎠ φ(q) = q5 − q2 = 0. q −q 6 3 i. Describe the constraints physically in terms of the behavior of the two particles. ii. How many degrees of freedom does the system have with this constraint imposed?

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:18:33, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.005

4

Conservation Principles

The traditional starting point in the classical study of mechanics is with the so-called conservation principles. Newton’s famous laws of motion form the basis of classical mechanics. We can consider Newton’s second law as part of a more general conservation principle; specifically the conservation of translational momentum. In this chapter we will begin with Newton’s laws, which address point masses, and generalize to extended bodies (rigid bodies) using Euler’s extension of Newton’s laws. In both cases, point masses and rigid bodies, conservation of momentum plays the central role. We will articulate the Newton-Euler Principle as the basis for deriving the equations of motion for both a single rigid body and for systems of interconnected rigid bodies (i.e., kinematic chains). As we apply the conservation of translational and angular momentum to a single rigid body, we will define a set of fundamental inertial properties, including the center of mass and the inertia tensor. Furthermore, as we apply the conservation principles to a set of interconnected rigid bodies, we will articulate the iterative Newton-Euler method for deriving the equations of motion of kinematic chains. A fundamental concept in Newtonian mechanics is that of an inertial (Galilean) reference frame. This is a frame of reference in which time and space are homogeneous and isotropic; that is, the laws of physics are the same for all points and orientations. All inertial references frames have constant motion (constant translational velocity) with respect to each other. Derivatives will be assumed to be taken with respect to an inertial reference frame, unless otherwise stated.

4.1

The Newton-Euler Principle We begin by addressing a single point mass. Newton’s laws of motion are as follows: 1. With respect to an inertial reference frame, a point mass at rest tends to stay at rest. A point mass in motion tends to move at a constant velocity unless acted on by external forces. 2. The sum of forces, f , acting on a point mass impart to it an acceleration, a, in the same direction as the resultant force vector and with a magnitude inversely proportional to the mass, M, of the body. That is, f = Ma.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

84

Conservation Principles

3. When one point mass exerts a force on another point mass, the latter point mass exerts a force of equal magnitude and opposite direction on the former point mass. This is also known as the weak law of action and reaction. Newton’s second law can be more generally stated in terms of translational momentum: p r i n c i p l e 4.1 The translational momentum of a closed system is conserved, dp = 0. dt For a system with externally applied forces we have

(4.1)

dp  = fi . dt i=1 nf

(4.2)

This is known as Newton’s Principle. Newton’s third law implies that the force that body A exerts on body B is equal and opposite to the force that body B exerts on body A. That is, fBA = − fAB .

(4.3)

Euler extended Newton’s second law to rigid bodies. This extension can be generally stated as follows: p r i n c i p l e 4.2 The translational and angular momenta of a closed system are conserved: dp dH = 0 and = 0. (4.4) dt dt For a system with externally applied forces and moments we have dp  = fi dt i=1 nf

  dH G = d−GP→i × fi + ϕ j. dt i=1 j=1 nf

and,

nϕ

(4.5)

This is known as the Newton-Euler Principle. Euler’s extension of Newton’s third law implies that the moment that body A exerts on body B is equal and opposite to the moment that body B exerts on body A. That is, ϕBA = −ϕAB .

4.1.1

(4.6)

A Single Particle Newton’s Principle for a single point mass with a discrete set of n f external forces, { f1 , . . . , fn f }, (and gravity) acting on it is expressed as dp  fi − Mgeˆ3 , = dt i=1 nf

(4.7)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

4.1 The Newton-Euler Principle

85

Figure 4.1 Free-body diagram showing external forces, fi , and moments, ϕ j , and the time rates of change of translational and angular momenta, p˙ and H˙ G , respectively. The momenta are relative to the center of mass, G.

where translational momentum is defined as p  Mv.

(4.8)

Thus, Ma =

nf 

fi − Mgeˆ3 .

(4.9)

i=1

4.1.2

A Single Rigid Body The Newton-Euler principle for a single rigid body with a discrete set of n f external forces, { f1 , . . . , fn f }, and nϕ external moments, {ϕ1 , . . . , ϕnϕ }, acting on it is expressed as dp  = fi − Mgeˆ3 dt i=1 nf

  dH G = d−GP→i × fi + ϕ j. dt i=1 j=1 nf

and

nϕ

(4.10)

The free-body diagram for this is illustrated in Figure 4.1. The center of mass position vector, rG , of a continuous rigid system is defined as

1 rG  r dM. (4.11) M M

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

86

Conservation Principles

The velocity of the center of mass, v G , with respect to an inertial reference frame is then vG =

d 1 rG = dt M

v dM,

(4.12)

M

and the acceleration of the center of mass, aG , with respect to an inertial reference frame is d 1 aG = v G = dt M

a dM.

(4.13)

M

Translational momentum is defined as

p

v dM = Mv G .

(4.14)

M

The derivative of p with respect to an inertial reference frame can then be expressed as d d p= dt dt



v dM = M

d v dM = dt

M

a dM.

(4.15)

M

Alternately, we can express the derivative of p as d d p = Mv G = MaG , dt dt

(4.16)

and by Principle 4.2,

MaG =

nf 

fi − Mgeˆ3 .

(4.17)

i=1

The angular momentum of the body about an arbitrary origin point, O, is defined as

HO 

r × v dM.

(4.18)

M

It will be useful to represent the vector r as the sum of the vector from the origin to the center of mass, rG , and the vector from the center of mass to a point on the body, ρ. Similarly, the vector v can be represented as the sum of the velocity of the center of mass, v G , and ω × ρ (see Figure 4.2). Thus, the angular momentum can be expressed in

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

87

4.1 The Newton-Euler Principle

Figure 4.2 Angular momentum of a body about an arbitrary origin point is depicted. The vector

from the origin to a point on the body, r, is composed of the vector from the origin to the center of mass, rG , and the vector from the center of mass to a point on the body, ρ. The velocity vector of a point on the body, v, is composed of the velocity of the center of mass, v G , and ω × ρ.

terms of the center of mass and velocity of the center of mass,

O H = (rG + ρ) × (v G + ω × ρ) dM = rG × v G dM M

M

  

+ rG × (ω ×

ρ dM ) + M

  

= rG × Mv G +

M

MρG =0

ρ dM ×v G + M

  

ρ × (ω × ρ) dM M

MρG =0

ρ × (ω × ρ) dM.

(4.19)

M

Noting the identity a × (b × c) = b(a · c) − c(a · b),

(4.20)

we have 



ρ × (ω × ρ) = ω(ρ · ρ) − ρ(ρ · ω) = ρ2 ω − ρρT ω = ρ2 1 − ρρT ω. (4.21) So, the angular momentum can be expressed as ⎤ ⎡

 ρ2 1 − ρρT dM ⎦ ω. (4.22) H O = rG × Mv G + ⎣ M

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

88

Conservation Principles

We define the inertia tensor evaluated about the center of mass:



 ρ2 1 − ρρT dM = [(ρ · ρ)1 − ρ ⊗ ρ] dM. IG  M

(4.23)

M

The inertia tensor is symmetric positive definite, a fact that we will not show here. We have H O = rG × Mv G + I G ω,

(4.24)

or, noting that v G = v O + ω × rG , we have H O = MrG × v O + MrG × (ω × rG ) + I G ω = MrG × v O + M [(rG · rG )1 − rG ⊗ rG ] ω + I G ω

 = MrG × v O + I G + M[(rG · rG )1 − rG ⊗ rG ] ω.

(4.25)

If we evaluate the angular momentum about the center of mass, then

H G = ρ × (v G + ω × ρ) dM = ρ dM ×v G + ρ × (ω × ρ) dM = I G ω. M

M

  

M

MρG =0

(4.26) We note that I O = I G + M[(rG · rG )1 − rG ⊗ rG ].

(4.27)

This relationship is known as the parallel axis theorem. Furthermore, H O = rG × Mv G + H G = MrG × v O + I O ω.

(4.28)

Frame transformations for the inertia tensor can be accommodated with the familiar tensor double product. That is, I = ABQBI G BAQ.

A G

(4.29)

This can be demonstrated by noting that H G = BI G Bω.

(4.30)

H G = ABQBH G = ABQ(BI G Bω) = (ABQBI G BAQ)Aω = AI G Aω.

(4.31)

B

Therefore, A

Using the expression for angular momentum given in (4.18), the derivative of H O with respect to an inertial reference frame can be expressed as

d O d d H = r × v dM = (v × v + r × a) dM r × v dM = (4.32) dt dt dt M M M

(4.33) = r × a dM. M

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

89

4.1 The Newton-Euler Principle

More rigorously, we can see that d (r + r) × (v + v) − r × v (r × v) = lim = r × a. t→0 dt t

(4.34)

Expanding this expression for the derivative of H O , we can express it in terms of the center of mass and the acceleration of the center of mass:

d O H = r × a dM = (rG + ρ) × [aG + α × ρ + ω × (ω × ρ)] dM dt M M

= rG × aG dM +rG × (α × ρ dM ) + rG × [ω × (ω × ρ dM )] M

M

  

M

ρ dM ×aG +

+ M

  

MρG =0

ρ × (α × ρ) dM + M

ρ × [ω × (ω × ρ)]dM M

ρ × (α × ρ) dM +

M

  

MρG =0

MρG =0

= rG × MaG +

M

  

ρ × [ω × (ω × ρ)]dM.

(4.35)

M

We have 



ρ × (α × ρ) = α(ρ · ρ) − ρ(ρ · α) = ρ2 α − ρρT α = ρ2 1 − ρρT α, (4.36) and ρ × [ω × (ω × ρ)] = ρ × [ω(ω · ρ) − ρ(ω · ω)] = ρ × ω(ω · ρ) = −ω × ρ(ω · ρ) = ω × (−ρT ρ)ω = ω × (−ρT ρ)ω + ω × ρ2 ω = ω × [(−ρT ρ)ω + ρ2 ω] = ω × (ρ2 1 − ρT ρ)ω.

(4.37)

So, ⎡ ⎤

 d O ρ2 1 − ρρT dM ⎦ α H = rG × MaG + ⎣ dt M ⎡ ⎤

 ρ2 1 − ρρT dM ⎦ ω, (4.38) +ω×⎣ M

or d O H = rG × MaG + I G α + ω × I G ω. dt

(4.39)

Alternately, by using the expression for angular momentum from (4.24), we can express the derivative of H O as d O d d d H = (rG × Mv G + I G ω) = (rG × Mv G ) + I G ω, dt dt dt dt

(4.40)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

90

Conservation Principles

where (rG + rG ) × M (v G + v G ) − rG × Mv G d (rG × Mv G ) = lim t→0 dt t = rG × MaG

(4.41) (4.42)

and (1 + Q)I G (ω + ω) − I G ω d G I ω = lim = I G α + ω × I G ω. t→0 dt t

(4.43)

So, d O H = rG × MaG + I G α + ω × I G ω. dt

(4.44)

Evaluated about the center of mass (rG = 0), we have d G H = I G α + ω × I G ω, dt

(4.45)

and by Principle 4.2, I Gα + ω × I Gω =

nf 

d−GP→i × fi +

i=1

nϕ 

ϕ j.

(4.46)

j=1

Example: We consider the mass properties of the cone depicted in Figure 4.3. The radius of the base is R and the height is h in the z direction. Using cylindrical coordinates, the infinitesimal volume and mass elements are dV = r dθ dr dz

and

dM = ρ r dθ dr dz,

(4.47)

where ρ is the material density (assumed to be a constant throughout the body). The position vector of a point in the body can be represented as

T r = r cos(θ ) r sin(θ ) z .

(4.48)

Using the coordinate frame at the vertex of the cone, we can integrate to compute the total mass. The integration limits in the radial direction vary linearly with the z coordinate. Specifically, the outer radius of the cone is Ro = zR/h. The mass integral is then

M=

dM =

= 0

h

h

0 2 2

zR/h 0

2π

h

ρ r dθ dr dz =

0

ρπz R 1 dz = ρ h π R2 . 2 h 3

0

zR/h

2ρ π r dr dz

0

(4.49)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

91

4.1 The Newton-Euler Principle

Figure 4.3 A cone with a base radius of R and height of h in the z direction.

The center of mass computed with respect to the vertex is

T 1 1 h zR/h 2π

r dM = r cos(θ ) r sin(θ ) z ρ r dθ dr dz M M 0 0 0

T 1 h zR/h

= 0 0 ρ 2π r z dr dz M 0 0

T T 1 h 1

2 3 = dz = (4.50) 0 0 14 ρ π R2 h2 . 0 0 ρ πhR2 z M 0 M

rG =

Since M = 13 ρ h π R2 , we have

rG = 0

T 3 h 4

0

.

(4.51)

The inertia tensor computed about the vertex is

[(r · r)1 − r ⊗ r] dM =

IO =

h

0

M

zR/h 0

2π

[(r · r)1 − r ⊗ r] ρ r dθ dr dz. (4.52)

0

We note that ⎛

r2 cos2 (θ )

⎜ 2 r cos(θ ) sin(θ ) (r · r)1 − r ⊗ r = (r2 + z2 )1 − ⎜ ⎝ rz cos(θ ) ⎛

z2 + r2 sin2 (θ )

⎜ = ⎝ −r2 cos(θ ) sin(θ ) −rz cos(θ )

r2 cos(θ ) sin(θ ) rz cos(θ )

⎟ rz sin(θ ) ⎟ ⎠ z2

r2 sin2 (θ ) rz sin(θ )

−r2 cos(θ ) sin(θ ) −rz cos(θ ) z2 + r2 cos2 (θ ) −rz sin(θ )

⎞

⎞

⎟ −rz sin(θ ) ⎠ . r

(4.53)

2

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

92

Conservation Principles

So,

h

IO = 0

zR/h

0

⎛

0 h

=

2π 0

⎞

z2 + r2 sin2 (θ ) −r2 cos(θ ) sin(θ ) −rz cos(θ ) 2 ⎝ −r cos(θ ) sin(θ ) z2 + r2 cos2 (θ ) −rz sin(θ ) ⎠ρ r dθ −rz cos(θ ) −rz sin(θ ) r2

⎛ zR/h

0

ρ π r(r2 + 2z2 ) ⎝ 0 0

⎛ ρ π (4h2 R2 +R4 )z4

h

= 0

⎛ ⎜ =⎝

⎜ ⎜ ⎝

1 ρ 20

⎞

0 0 2 2 0 ⎠dr dz ρ π r(r + 2z ) 0 ρ 2π r3

⎞

4h4

0

0

0

ρ π (4h2 R2 +R4 )z4 4h4

0

0

0

ρ πR4 z4 2h4

h π R2 (4h2 + R2 ) 0

⎛ ⎜ IO = ⎝

⎟ ⎟ dz ⎠

0 1 2 ρ h π R (4h2 + R2 ) 20

0 or

1 ρ 10

+ R2 )

0 3 M (4h2 20

0 0

⎞

0 0

0 3 M (4h2 20

dr dz

⎟ ⎠,

⎞

0

⎟ 0 ⎠. 3 2 R 10

+ R2 )

0

(4.54)

h π R4

(4.55)

We can compute the inertia tensor about the center of mass. The limits on the middle and outer integrals change to reflect the change in coordinates,

h/4 zR/h+3R/4 G I = −3h/4

0

⎛

2π 0

⎛ ⎜ =⎝

1 ρ 80

⎞

z2 + r2 sin2 (θ ) −r2 cos(θ ) sin(θ ) −rz cos(θ ) ⎝ −r2 cos(θ ) sin(θ ) z2 + r2 cos2 (θ ) −rz sin(θ ) ⎠ρ r dθ −rz cos(θ ) −rz sin(θ ) r2

h π R2 (h2 + 4R2 ) 0

0 1 2 2 ρh π R (h + 4R2 ) 80

0 or

⎛ ⎜ IG = ⎝

0 + 4R2 ) 0

3 M (h2 80

0 3 2 M (h + 4R2 ) 80

0 We note that

0 ⎛

⎜ M[(rG · rG )1 − rG ⊗ rG ] = ⎝

⎞

0 0 1 ρ 10

⎟ ⎠,

(4.56)

⎞ 0 0 ⎟ ⎠. 3 2 R 10

(4.57)

h π R4

9 2 hM 16

0

0 0

9 2 h M 16

0

dr dz

0

⎞

⎟ 0⎠ 0

(4.58)

and verify that I = I + M[(rG · rG )1 − rG ⊗ rG ]. O

G

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

93

4.1 The Newton-Euler Principle

4.1.3

A System of Particles Newton’s Principle applied to a system of n p unconstrained particles with mutual forces of interaction between them yields  d pi fi j − Mi geˆ3 = dt j=1 np

(4.59)

for i = 1, . . . , n p . The term fi j is the force that particle j exerts on particle i. Thus, Mi ai =

np 

fi j − Mi geˆ3 ,

(4.60)

j=1

where fii = 0 and, by Newton’s third law, f ji = − fi j .

4.1.4

A System of Rigid Bodies We now turn our attention to a system of rigid bodies forming a serial chain. Performing a force balance on link i, we have i

fii−1 + i fii+1 = Mi (iaGi + gieˆ03 ).

(4.61)

By Newton’s third law, the reaction forces between links are equal and opposite. Thus, i fii+1 = −i fi+1 , so

i

i fii−1 − i fi+1 = Mi (iaGi + gieˆ03 ),

(4.62)

i fii−1 = Mi (iaGi + gieˆ03 ) + ii+1Qi+1 fi+1 .

(4.63)

i

or i

Performing a moment balance on link i about the center of mass, we have i

i i+1 −−→ × f ϕii−1 + iϕii+1 + id−G→i i × i fii−1 + id−G−i−(i+1) = iIiGi iαi + iωi × iIiGi iωi . i

(4.64)

Based on Euler’s extension of Newton’s third law to moments, the reaction moments i , so between links are equal and opposite. Thus, iϕii+1 = −iϕi+1 i

i i i −−→ × f ϕii−1 − iϕi+1 + id−G→i i × i fii−1 − id−G−i−(i+1) = iIiGi iαi + iωi × iIiGi iωi , i+1

(4.65)

or i i i −−→ × ϕii−1 = iIiGi iαi + iωi × iIiGi iωi + ii+1Qi+1ϕi+1 − id−G→i i × i fii−1 + id−G−i−(i+1) Qi+1 fi+1 . i+1

i

(4.66)

Alternately, performing a moment balance on link i about the proximal point of link i, we have i i i+1 −−−→ × f ϕii−1 − iϕi+1 − id−iG→i × Mi (iaGi + gieˆ03 ) + idi(i+1) = iIiGi iαi + iωi × iIiGi iωi , (4.67) i

i

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

94

Conservation Principles

Algorithm 2 Iterative Newton-Euler method 1:

0

ω0 = 0 {initialization}

2:

0

α0 = 0 {initialization}

3:

0

a0 = 0 {initialization}

4: 5: 6:

for i = 1 to n do if ji = 1 {joint is revolute} then i ωi = ii−1Qi−1ωi−1 + θ˙i izˆi i

7:

αi = ii−1Qi−1αi−1 + θ¨i izˆi + ii−1Qi−1ωi−1 × θ˙i izˆi i

8:

−−→ ai = ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i −−→ )] + i−1ωi−1 × (i−1ωi−1 × i−1d−(i−1)i

i

9:

aGi = iai + iαi × id−iG→i + iωi × (iωi × id−iG→i )

10:

else

11:

i

ωi = ii−1Qi−1ωi−1

12:

i

αi = ii−1Qi−1αi−1 −−→ ai =ii−1Q[i−1ai−1 + i−1αi−1 × i−1d−(i−1)i

i

13:

i i−1 −−→ )] + 2 Q + i−1ωi−1 × (i−1ωi−1 × i−1d−(i−1)i ωi−1 × d˙i izˆi + d¨i izˆi i−1 i

14:

aGi = iai + iαi × id−iG→i + iωi × (iωi × id−iG→i )

end if

15: 16:

end for

17:

n+1

18:

n+1

19: 20: 21:

n fn+1 = 0 {initialization}

n ϕn+1 = 0 {initialization}

for i = n to 1 do if ji = 1 {joint is revolute} then i

i fii−1 = Mi (iaGi + gieˆ03 ) + ii+1Qi+1 fi+1 i ϕii−1 = iIiGi iαi + iωi × iIiGi iωi + ii+1Qi+1ϕi+1 − id−G→i i × i fii−1

i

22: 23:

i i −−→ × + id−G−i−(i+1) Qi+1 fi+1 i+1

τi = izˆi · iϕii−1

24:

else

25:

i

26: 27: 28:

i fii−1 = Mi (iaGi + gieˆ03 ) + ii+1Qi+1 fi+1

τi = izˆi · i fii−1 end if end for

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

4.1 The Newton-Euler Principle

95

or i i i −−−→ × ϕii−1 = iIiGi iαi + iωi × iIiGi iωi + ii+1Qi+1ϕi+1 + id−iG→i × Mi (iaGi + gieˆ03 ) + idi(i+1) Qi+1 fi+1 . i+1 (4.68)

i

The iterative Newton-Euler method consists of n outward iterations that propagate the kinematic quantities of the system along the chain. These kinematic quantities are given by (3.216), (3.219) through (3.222), and (3.225) through (3.227). This is followed by n inward iterations that propagate the kinetic quantities of the system along the chain. These kinetic quantities are given by (4.63) and (4.66). The applied joint forces and moments in the local ixˆi and iyˆi directions constitute reaction forces and moments. The applied joint forces and moments in the local izˆi direction constitute forces/moments, τi , associated with joint actuators. The joint forces/moments are given by τi = izˆi · iϕii−1

(4.69)

τi = izˆi · i fii−1

(4.70)

for revolute joints and by,

for prismatic joints. The iterative Newton-Euler method is summarized in Algorithm 2.

Example: We consider a serial chain robot. We parameterize joint angles, as shown in Figure 4.4. The rotation matrices are ⎛ ⎞ cos(θ1 ) − sin(θ1 ) 0 0 Q = Qz (θ1 ) = ⎝ sin(θ1 ) cos(θ1 ) 0 ⎠ , 1 0 0 1 ⎛ cos(θ2 ) − sin(θ2 ) 1 ⎝ Q = Q (π /2)Q (θ ) = 0 0 x z 2 2 sin(θ2 ) cos(θ2 ) ⎛ ⎞ cos(θ3 ) − sin(θ3 ) 0 2 Q = Qz (θ3 ) = ⎝ sin(θ3 ) cos(θ3 ) 0 ⎠ . 3 0 0 1

the system using three

(4.71) ⎞ 0 −1 ⎠ , 0

(4.72)

(4.73)

The outward kinematic propagation equations are computed. For link 1 we have ⎛ ⎞ 0 1 ω1 = θ˙1 1zˆ1 = ⎝ 0 ⎠ (4.74) ˙ θ1 ⎛ ⎞ 0 1 α1 = θ¨i 1zˆ1 = ⎝ 0 ⎠ (4.75) θ¨1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

96

Conservation Principles

Figure 4.4 A 3 degree-of-freedom serial chain robot parameterized by the joint angles θ1 , θ2 , and θ3 . The link lengths are l1 , l2 , and l3 , the radii are r1 , r2 , and r3 , and the masses of the links are M1 , M2 , and M3 .

and a1 = 0

1

aG1 = 1a1 + 1α1 ×

1

l1 eˆ1 + 1ω1 × 2

 1

ω1 ×

l1 eˆ1 2

⎛

 =

−θ˙12

⎞

l1 ⎝ ¨ ⎠ . θ1 2 0

(4.76) (4.77)

For link 2 we have ⎞ sin(θ2 )θ˙1 ⎟ ⎜ 2 ω2 = 21Q1ω1 + θ˙2 2zˆ2 = ⎝ cos(θ2 )θ˙1 ⎠ θ˙2 ⎛

(4.78)

⎞ cos(θ2 )θ˙1 θ˙2 + sin(θ2 )θ¨2 ⎟ ⎜ 2 α2 = 21Q1α1 + θ¨2 2zˆ2 + 21Q1ω1 × θ˙2 2zˆ2 = ⎝ −sin(θ2 )θ˙1 θ˙2 + cos(θ2 )θ¨2 ⎠ θ¨2 ⎛

(4.79)

and ⎛

⎞ −cos(θ2 )θ˙12 ⎜ ⎟ 2 a2 = 21Q[1a1 + 1α1 × l1 eˆ1 + 1ω1 × (1ω1 × l1 eˆ1 )] = l1 ⎝ sin(θ2 )θ˙12 ⎠ , −θ¨1   l2 l2 2 aG2 = 2a2 + 2α2 × eˆ1 + 2ω2 × 2ω2 × eˆ1 , 2 2 ⎛ ⎞ ˙ − cos(θ2 )[2l1 + l2 cos(θ2 )]θ12 − l2 θ˙22 1⎜ ⎟ = ⎝ sin(θ2 )[2l1 + l2 cos(θ2 )]θ˙12 + l2 θ¨2 ⎠ . 2 2l2 sin(θ2 )θ˙1 θ˙2 − l2 [2l1 + l2 cos(θ2 )]θ¨1

(4.80)

(4.81)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

4.1 The Newton-Euler Principle

For link 3 we have

⎞ sin(θ2 + θ3 )θ˙1 ⎟ ⎜ 3 cos(θ2 + θ3 )θ˙1 ⎟ ω3 = 32Q2ω2 + θ˙3 3zˆ3 = ⎜ ⎠ ⎝ θ˙2 + θ˙3

97

⎛

(4.82)

α3 = 32Q2α2 + θ¨3 3zˆ3 + 32Q2ω2 × θ˙3 3zˆ3 ⎞ ⎛ cos(θ2 + θ3 )θ˙1 (θ˙2 + θ˙3 ) + sin(θ2 + θ3 )θ¨1 ⎟ ⎜ − sin(θ2 + θ3 )θ˙1 (θ˙2 + θ˙3 ) + cos(θ2 + θ3 )θ¨1 ⎟ =⎜ ⎠ ⎝ θ¨2 + θ¨3

3

(4.83)

and a3 = 32Q[2a2 + 2α2 × l2 eˆ1 + 2ω2 × (2ω2 × l2 eˆ1 )] ⎞ ⎛ −[l1 + l2 cos(θ2 )] cos(θ2 + θ3 )θ˙12 − l2 [cos(θ3 )θ˙22 − sin(θ3 )θ¨2 ] ⎟ ⎜ ˙2 ˙2 ¨ ⎟ =⎜ ⎝ [l1 + l2 cos(θ2 )] sin(θ2 + θ3 )θ1 + l2 [sin(θ3 )θ2 + cos(θ3 )θ2 ] ⎠ 2l2 sin(θ2 )θ˙1 θ˙2 − [l1 + l2 cos(θ2 )]θ¨1

3

aG3 = 3a3 + 3α3 ×

3

⎛1 2

l3 eˆ1 + 3ω3 × 2

 3

ω3 ×

l3 eˆ1 2

(4.84)



[−2 cos(θ2 + θ3 )[2l1 + 2l2 cos(θ2 )] + l3 cos(θ2 + θ3 )θ˙12

1⎜ 2 = ⎝ [2l1 + 2l2 cos(θ2 ) + l3 cos(θ2 + θ3 )] sin(θ2 + θ3 )θ˙1 2 2θ˙1 [[2l2 sin(θ2 ) + l3 sin(θ2 + θ3 )]θ˙2 + l3 sin(θ2 + θ3 )θ˙3 ]

⎞

−2[[l3 + 2l2 cos(θ3 )]θ˙22 + l3 θ˙32 − 2l2 sin(θ3 )θ¨2 ]] +2l2 sin(θ3 )θ˙22 + l3 θ¨2 + 2l2 cos(θ3 )θ¨2 + l3 θ¨3 ⎟ . −[2l1 + 2l2 cos(θ2 ) + l3 cos(θ2 + θ3 )]θ¨1

⎠

(4.85)

Now we can compute the inward kinetic propagation equations. First, the inertia tensors are ⎞ ⎛ 1 M r2 0 0 2 i i ⎟ ⎜ i Gi 1 2 1 2 ⎟. (4.86) Ii = ⎜ 0 ⎠ ⎝ 0 12 li Mi + 4 Mi ri 1 2 1 2 l Mi + 4 Mi ri 0 0 12 i For link 3 we have 3

f32 = M3 (3aG3 + g3eˆ03 ),

l3 G G 3 2 ϕ3 = 3I3 3 3α3 + 3ω3 × 3I3 3 3ω3 + eˆ1 × 3 f32 , 2 1 3 3 2 M3 [6gl3 cos(θ2 + θ3 ) τ3 = zˆ3 · ϕ3 = 12 +[6l1 l3 + 6l2 l3 cos(θ2 ) + (4l32 − 3r32 ) cos(θ2 + θ3 )] sin(θ2 + θ3 )θ˙12  +6l2 l3 sin(θ3 )θ˙22 + [4l32 + 3r32 + 6l2 l3 cos(θ3 )]θ¨2 + (4l32 + 3r32 )θ¨3 .

(4.87) (4.88)

(4.89)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

98

Conservation Principles

Figure 4.5 (Top) Animation frames from the simulation of a serial chain robot. No actuator torque was specified, but an initial velocity condition of π /10 rad/s (18 deg/s) was specified on joint 1 to provide the spin about the vertical axis. The trace records the trajectory of the endpoint of the chain. (Bottom) Time history of the joint angles θ1 , θ2 , and θ3 , parameterizing the configuration of the mechanism.

For link 2 we have 2

f21 = M2 (2aG2 + g2eˆ03 ) + 23Q3 f32 ,

2

ϕ21 = 2I2 2 2α2 + 2ω2 × 2I2 2 2ω2 + 23Q3ϕ32 + G

τ2 = 2zˆ2 · 2ϕ21 ,

G

(4.90) l2 l2 eˆ1 × 2 f21 + eˆ1 × 23Q3 f32 , 2 2

(4.91) (4.92)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

4.2 Exercises

99

and for link 1 we have 1

f10 = M1 (1aG1 + g1eˆ03 ) + 12Q2 f21 ,

1

ϕ10 = 1I1 1 1α1 + 1ω1 × 1I1 1 1ω1 + 12Q2ϕ21 + G

G

(4.93) l1 l1 eˆ1 × 1 f10 + eˆ1 × 12Q2 f21 , 2 2

τ1 = 1zˆ1 · 1ϕ10 .

(4.94) (4.95)

The number of terms quickly grows for links 2 and 1, so we will not express them here. The simulation results are displayed in Figure 4.5. The following values were used for the constants: r1 = 0.125,

r2 = 0.1,

r3 = 0.09,

l1 = 1,

l2 = 0.75,

l3 = 0.5,

M1 = 1,

M2 = 0.75,

M3 = 0.5.

(4.96)

The initial conditions used were 

π π T θo = 0 − 2.5 − 12

π T θ˙o = 10 0 0 .

4.2

(4.97) (4.98)

Exercises 1. Consider the cylindrical tube with an inside radius of r1 , an outside radius of r2 , and a height of h in the z direction (see Figure 4.6). (a) Compute the mass using a uniform material density of ρ. Use the coordinate system associated with the point O. (b) Compute the center of mass. Again, use the coordinate system associated with the point O. (c) Compute the inertia tensor about point O, I O .

Figure 4.6 (Left) Cylindrical tube with an inside diameter of r1 , an outside diameter of r2 , and a height of h in the z direction (Exercise 1). (Right) Cuboid with a length of l in the x direction, a width of w in the y direction, and a height of h in the z direction (Exercise 2).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

100

Conservation Principles

(d) Compute the inertia tensor about the center of mass, I G . (e) Verify that I O = I G + M[(rG · rG )1 − rG ⊗ rG ]. 2. Repeat Exercise 1 for the cuboid with a length of l in the x direction, a width of w in the y direction, and a height of h in the z direction (see Figure 4.6). 3. Consider the serial chain robot from Section 3.5, Exercise 14 (shown in Figure 4.7). The link lengths are l1 and l2 . The link radii are r1 and r2 , the link masses are M1 and M2 , and the link inertia tensors are ⎞ ⎛ 1 M r2 0 0 2 i i ⎟ ⎜ i Gi 1 2 ⎟ Ii = ⎜ l Mi + 14 Mi ri2 0 12 i ⎠ ⎝ 0 1 2 1 2 0 0 l Mi + 4 Mi ri 12 i for links i = 1, 2.

Figure 4.7 A 2 degree-of-freedom serial chain robot with joint angles θ1 and θ2 and joint torques τ1 and τ2 (Exercise 3). The link lengths are l1 and l2 . The centers of mass are at the geometric centers of the links. The link radii are r1 and r2 , the link masses are M1 and M2 , and the link G G G principal inertia components are iIi11i = 12 Mi ri2 and iIi22i = iIi33i = 121 li2 Mi + 14 Mi ri2 for links i = 1, 2.

(a) If not already completed, compute the terms iωi , iαi , iai , and iaGi for i = 1, 2 using forward kinematic propagation. (b) Compute the terms i fii−1 , iϕii−1 , and τi for i = 1, 2 using backward propagation. (c) Solve the τ1 and τ2 expressions for θ¨1 and θ¨2 .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:28:42, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.006

5

Zeroth-Order Variational Principles

In the previous chapter we addressed principles based on the conservation of vector quantities, specifically translational and angular momentum. In this chapter we will address principles rooted in the variation of scalar quantities like work and energy. We will begin with d’Alembert’s Principle of Virtual Work, which is an extension of Bernoulli’s static principle to dynamics. The principle is based on the notion of virtual displacement. We will refer to d’Alembert’s Principle as a zeroth-order variational principle to denote that it is based on the variation of the zeroth-order derivative of displacement. This is in contrast to higher-order variations related to velocity and acceleration, which will be discussed in subsequent chapters.

5.1

Virtual Displacements Virtual displacements refer to all displacements of a system that satisfy the scleronomic constraints of the system. Scleronomic constraints refer to constraints that are not explicitly dependent on time, as opposed to rheonomic constraints, which are explicitly dependent on time. In the case of virtual displacements, time is frozen or stationary.

5.2

D’Alembert’s Principle of Virtual Work p r i n c i p l e 5.1 The virtual work of a system is stationary. That is, δW = 0.

(5.1)

Additionally, the constraints of the system perform no virtual work: δWc = 0.

(5.2)

This is known as d’Alembert’s Principle. It is noted that while d’Alembert’s Principle can be seen as providing an alternate statement of Newton’s second law, for interacting bodies, a law of action and reaction (Newton’s third law) is still needed. Therefore, when we use d’Alembert’s Principle to derive the equations of motion for systems of particles/bodies, we will invoke the law of action and reaction.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

102

Zeroth-Order Variational Principles

5.2.1

A Single Particle D’Alembert’s Principle for a single point mass with a discrete set of n f external forces, { f1 , . . . , fn f }, acting on it is expressed as δW =

nf 

fi · δr − Mgeˆ3 · δr − Ma · δr = 0,

i=1

(5.3)

∀δr ∈ R3 , where δr represents the displacement variations. During these variations time is stationary. That is, δt = 0. More concisely, we can express   nf  fi − Mgeˆ3 − Ma · δr = 0, δW = (5.4) i=1 ∀δr ∈ R3 , which implies nf 

fi − Mgeˆ3 − Ma = 0.

(5.5)

i=1

5.2.2

A Single Rigid Body D’Alembert’s Principle for a single rigid body with a discrete set of n f external forces, { f1 , . . . , fn f }, and nϕ external moments, {ϕ1 , . . . , ϕnϕ }, acting on it is expressed as δW =

nf 

fi · δrPi +

i=1

nϕ 

ϕ j · δθ − Mgeˆ3 · δrG − MaG · δrG − (I G α + ω × I G ω) · δθ = 0,

i=1

∀δrG ∈ R3 ,

and

∀δθ ∈ R3 ,

(5.6)

where the δr and δθ terms represent all displacement variations consistent with the rigid-body constraint. This is depicted in Figure 5.1. During these variations, time is stationary. That is, δt = 0. D’Alembert’s Principle states that the virtual work associated with all internal forces and moments consistent with the rigid body constraint is zero (Wc = 0). We further note that δrPi = δrG + δθ × d−GP→i .

(5.7)

Therefore, nf  i=1

fi · δrPi =

nf  i=1

 nf  nf   fi · (δrG + δθ × d−GP→i ) = fi · δrG + fi · (δθ × d−GP→i ). i=1

(5.8)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

103

Figure 5.1 A virtual displacement of a rigid body consisting of a translational, δrG , and rotational, δθ, displacement. We are concerned with all such displacement variations consistent with the rigid-body constraint. During these variations, time is stationary.

Since a · (b × c) = (c × a) · b, nf 

nf 

fi · (δθ × d−GP→i ) =

i=1

(d−GP→i × fi ) · δθ =

 nf 

i=1

 d−GP→i × fi · δθ.

(5.9)

i=1

So, nf 

 fi · δrPi =

i=1

nf 

 fi · δrG +

i=1

 nf 

 d−GP→i × fi · δθ.

(5.10)

i=1

Substituting (5.10) into (5.6), we get δW =

 nf 

 fi − Mgeˆ3 − MaG · δrG

i=1

+

 nf 

d−GP→i × fi +

i=1

nϕ 

 ϕ j − I α − ω × I ω · δθ = 0, G

G

(5.11)

i=1

or ⎛ ⎜ ⎜n ⎝ f i=1

nf

⎞ fi − Mgeˆ3 − MaG

i=1

d−GP→i × fi +

nϕ

ϕ j − I Gα − ω × I Gω

⎟ ⎟· ⎠



δrG δθ

 = 0,

(5.12)

i=1

∀δrG ∈ R3 ,

and

∀δθ ∈ R3 ,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

104

Zeroth-Order Variational Principles

which implies nf 

fi − Mgeˆ3 − MaG = 0

(5.13)

ϕ j − I G α − ω × I G ω = 0.

(5.14)

i=1 nf 

d−GP→i × fi +

i=1

5.2.3

nϕ  i=1

A System of Particles We now apply d’Alembert’s Principle to a system of n p particles with a discrete set of forces acting on them. The virtual work associated with a given particle i is given by ⎛ ⎞ np  δWi = ⎝ fi j ⎠ · δri − Mi (ai + geˆ3 ) · δri = 0, (5.15) j=0 ∀δri ∈ R3 , for i = 1, . . . , n p . The term fi j is the force that particle j exerts on particle i, where fi0 is the force exerted by ground (inertial reference frame) on the ith particle. Equation (5.15) is true for any particle i in the system, under all independent variations, δri . If we sum (5.15) over all particles, we obtain ⎛ ⎞ np np np np     j⎠ ⎝ δW = δWi = fi · δri − Mi (ai + geˆ3 ) · δri = 0. (5.16) i=1

i=1

j=0

i=1

The first summation is associated with the virtual work performed by the interparticle reaction forces. It will be useful to rearrange the summation so as to pair up the equal and opposite reaction forces, fi j and f ji . Doing this, we have ⎛ ⎞ np np n p −1 n p np np       ⎝ fi j ⎠ · δri = ( fi j · δri + f ji · δr j ) + fii · δri − f0j · δr0 . i=1

j=0

i=0 j=i+1

i=1

j=1

(5.17) Since fii = 0 and δr0 = 0, we have ⎛ ⎞ np np n p −1 n p     ⎝ fi j ⎠ · δri = ( fi j · δri + f ji · δr j ). i=1

j=0

We note that f ji = − fi j . Thus, ⎛ ⎞ np np n p −1 n p     ⎝ fi j ⎠ · δri = fi j · (δri − δr j ). i=1

j=0

(5.18)

i=0 j=i+1

(5.19)

i=0 j=i+1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

105

This reflects the virtual work done by interparticle reaction forces, summed over all particles. If we consider that the particles are subject to a set of holonomic constraints, the positions, ri , are not independent. In this case we will assume that the positions can be expressed in terms of a set of nq independent generalized coordinates, q. If we now consider only the variations, δri , that are consistent with the kinematic constraints, then (5.19) reflects a projection of the interparticle forces on the direction of the interparticle motion. D’Alembert’s Principle states that the virtual work associated with all forces orthogonal to the interparticle motion (reaction forces) is zero (Wc = 0) and only the generalized force acting in the direction of interparticle motion produces virtual work. Thus, n p −1

np  

fi j · (δri − δr j ) =

nq 

τi · δqi ,

(5.20)

Mi (ai + geˆ3 ) · δri = 0

(5.21)

i=0 j=i+1

i=1

and (5.16) can be expressed as nq 

τi · δqi −

np 

i=1

i=0

for all variations, δri , that are consistent with the kinematic constraints. Expressing the variations in terms of variations in the generalized coordinates, we have δri = i δq.

(5.22)

So, (5.21) can be expressed as τ · δq −

np 

Mi (ai + geˆ3 ) · (i δq) = 0,

i=1

(5.23)

∀δq ∈ R , n

or

% τ−

np 

& i (Mi ai + Mi geˆ3 ) · δq = 0 T

i=1

(5.24)

∀δq ∈ R , n

which implies τ=

np 

Ti (Mi ai + Mi geˆ3 ).

(5.25)

ai = i q, ¨

(5.26)

i=1

Noting that

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

106

Zeroth-Order Variational Principles

we have τ=

 np 

 Mi i i T

nq  Mi Ti eˆ3 . q¨ + g

i=1

(5.27)

i=1

Defining the system mass matrix, M (q), and the vector of generalized gravity forces, g(q), M (q) 

np 

Mi Ti i

(5.28)

i=1 np

 g(q)  g Mi Ti eˆ3 ,

(5.29)

i=1

we have τ = M (q)q¨ + g(q).

5.2.4

(5.30)

A System of Rigid Bodies We now apply d’Alembert’s Principle to a system of nb rigid bodies forming a serial chain, with a discrete set of forces acting on them. The virtual work associated with a given body i is given by δWi = fii−1 · δrii−1 + fii+1 · δrii+1 − Mi (aGi + geˆ03 ) · δrGi + ϕii−1 · δθi +ϕii+1 · δθi − (IiGi αi + ωi × IiGi ωi ) · δθi = 0, ∀δrGi ∈ R , 3

and

(5.31)

∀δθi ∈ R , 3

for i = 1, . . . , nb . The term fii−1 is the force that body i − 1 exerts on body i. Likewise, ϕii−1 is the moment that body i − 1 exerts on body i. The term rii−1 is the point on body i to which body i − 1 attaches. Equation (5.31) is true for any rigid body i in the system, under all independent variations, δrGi and δθi (we note that δrii−1 and δrii+1 are functions of δrGi and δθi due to the rigid-body constraint). If we sum (5.31) over all rigid bodies, we obtain δW =

nb  i=1

δWi =

nb 

( fii−1 · δrii−1 + fii+1 · δrii+1 )

i=1

−

nb  i=1

−

nb 

Mi (aGi + geˆ03 ) · δrGi +

nb 

(ϕii−1 · δθi + ϕii+1 · δθi )

i=1

(IiGi αi + ωi × IiGi ωi ) · δθi = 0.

(5.32)

i=1

The first summation is associated with the virtual work performed by the interlink reaction forces. It will be useful to rearrange the summation so as to pair up the equal

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

107

i and opposite reaction forces, fii−1 and fi−1 , acting through each joint i. Doing this, we have

nb 

( fii−1 · δrii−1 + fii+1 · δrii+1 ) =

nb 

i=1

i i ( fii−1 · δrii−1 + fi−1 · δri−1 ) + fnnbb +1 · δrnnbb +1 − f01 · δr01 .

i=1

(5.33) Since fnnbb +1 = 0 and δr01 = 0, we have nb 

( fi

i−1

· δr

i−1 i

+ fi

i+1

· δr ) = i+1 i

i=1

nb 

i i ( fii−1 · δrii−1 + fi−1 · δri−1 ).

(5.34)

i=1

i We note that fi−1 = − fii−1 . Thus,

nb 

( fii−1 · δrii−1 + fii+1 · δrii+1 ) =

nb 

i=1

i fii−1 · (δrii−1 − δri−1 ).

(5.35)

i=1

This reflects the virtual work done by reaction forces at each joint, summed over all joints. As with the reaction forces, it will be useful to rearrange the summation associated with the virtual work performed by the interlink reaction moments so as to pair up the i , acting about each joint. Using the equal and opposite reaction moments, ϕii−1 and ϕi−1 same procedure as with the reaction forces, we have nb 

(ϕii−1 · δθi + ϕii+1 · δθi ) =

i=1

nb 

i (ϕii−1 · δθi + ϕi−1 · δθi−1 )

i=1

=

nb 

ϕii−1 · (δθi − δθi−1 ).

(5.36)

i=1

This reflects the virtual work done by reaction moments at each joint, summed over all joints. So, the total virtual work performed by the interlink reaction forces and moments can be expressed compactly as nb  i=1

( fii−1 · δrii−1 + fii+1 · δrii+1 ) +

nb 

(ϕii−1 · δθi + ϕii+1 · δθi )

i=1

=

 nb  i−1   i−1 i  fi δri − δri−1 · . ϕii−1 δθi − δθi−1

(5.37)

i=1

i If we now consider only the variations δrii−1 , δri−1 , δθi , and δθi−1 that are consistent with the kinematic constraints, then (5.37) reflects a projection of the interlink forces and moments on the direction of the joint motion. D’Alembert’s Principle states that the virtual work associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Wc = 0) and only the generalized force acting in the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

108

Zeroth-Order Variational Principles

direction of joint motion produces virtual work. Thus, nb  i−1   fi · ϕii−1



i=1

i δrii−1 − δri−1

 =

δθi − δθi−1

nq 

τi · δqi ,

(5.38)

i=1

and (5.32) can be expressed as nq 

τi · δqi −

i=1

nb 

Mi (aGi + geˆ03 ) · δrGi −

i=1

nb 

(IiGi αi + ωi × IiGi ωi ) · δθi = 0

(5.39)

i=1

for all variations, δrGi and δθi , that are consistent with the kinematic constraints. Using d’Alembert’s Principle, we can address a system of nb rigid bodies forming a branching chain in a similar manner. With a serial chain, the parent/child structure is implicit to the numbering scheme. Every link i has a single parent link, i − 1, at its proximal end and a single child link, i + 1, at its distal end (except for the nth link). The ith joint is at the proximal end of the ith link. With a branching chain the numbering of links is more arbitrary, without a parent/child structure implicit to the numbering scheme. We can explicitly capture the parent/child structure, however, by defining three additional parameters, λi , ci , μi j . The term λi is the parent link number of the ith link, ci is the number of child links for the ith link, and μi1 · · · μici are the child link numbers of the ith link. Given these parameters, the virtual work associated with a given body i is given by δWi = fiλi · δriλi +

ci 

fi

μi j

μ

· δri i j − Mi (aGi + geˆ03 ) · δrGi + ϕiλi · δθi

j=1

+

ci 

(5.40)

μ

ϕi i j · δθi − (IiGi αi + ωi × IiGi ωi ) · δθi = 0,

j=1

∀δrGi ∈ R3 ,

∀δθi ∈ R3 ,

and

λ

for i = 1, . . . , nb . The term ri i is the point on body i to which body (parent) λi attaches, μic μ and likewise ri i1 · · · ri i are the points on body i to which bodies (children) μi1 · · · μici μic μ λ attach. We note that δri i and δri i1 · · · δri i are functions of δrGi and δθi due to the rigidbody constraint. If we sum (5.40) over all rigid bodies, we obtain

δW =

nb  i=1

δWi =

nb 

⎛ ⎝ fiλi · δriλi +

i=1

−

nb  i=1

−

nb 

ci 

⎞ fi

μi j

· δri ⎠ μi j

j=1

Mi (aGi + geˆ03 ) · δrGi +

nb 

⎛ ⎝ϕiλi · δθi +

i=1

(IiGi αi + ωi × IiGi ωi ) · δθi = 0.

ci 

⎞ ϕi · δθi ⎠ μi j

j=1

(5.41)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

109

The term associated with the virtual work performed by the interlink reaction forces is ⎛ ⎞ nb ci   μi j μi j λ λ ⎝ fi i · δri i + fi · δri ⎠. (5.42) i=1

j=1

It will be useful to rearrange the summation so as to pair up the equal and opposite λ reaction forces, fi i and fλii , acting through each joint i. We can rewrite the summation of (5.42) based on considering the virtual work at each joint. The sum over all joints then gives us nb 

nb 

( fiλi · δriλi + fλii · δrλi i ) =

i=1

fiλi · (δriλi − δrλi i ).

(5.43)

i=1

The term associated with the virtual work performed by the interlink reaction moments is ⎛ ⎞ nb ci   μ ⎝ϕiλi · δθi + (5.44) ϕi i j · δθi ⎠. i=1

j=1

In a similar manner as before, we can rewrite this summation based on considering the virtual work at each joint. The sum over all joints then gives us nb 

nb 

(ϕ · δθi + ϕ · δθλi ) = λi i

i λi

i=1

ϕiλi · (δθi − δθλi ).

(5.45)

i=1

So, the total virtual work performed by the interlink reaction forces and moments can be expressed compactly as ⎛ ⎞ ⎛ ⎞ nb ci nb ci     μ μ μ ⎝ fiλi · δriλi + ⎝ϕiλi · δθi + fi i j · δri i j ⎠ + ϕi i j · δθi ⎠ i=1

j=1

i=1

=

nb   i=1

j=1

   λi δri − δrλi i fi · . λ ϕi i δθi − δθλi λi

(5.46)

λ

If we now consider only the variations δri i , δrλi i , δθi , and δθλi that are consistent with the kinematic constraints, then (5.46) reflects a projection of the interlink forces and moments on the direction of the joint motion. D’Alembert’s Principle states that the virtual work associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Wc = 0) and only the generalized force acting in the direction of joint motion produces virtual work. Thus,  λ  nq nb  λi    δri i − δrλi i fi · τi · δqi , (5.47) = λ ϕi i δθi − δθλi i=1 i=1 and (5.41) can be expressed as nq  i=1

τi · δqi −

nb  i=1

Mi (aGi + geˆ03 ) · δrGi −

nb 

(IiGi αi + ωi × IiGi ωi ) · δθi = 0,

(5.48)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

110

Zeroth-Order Variational Principles

for all variations, δrGi and δθi , that are consistent with the kinematic constraints. Expressing the variations in terms of variations in the generalized coordinates, we have i

δrGi = iGi δq

i

and

δθi = ii δq,

(5.49)

where the terms are expressed in the local link frame i for convenience. So, (5.48) can be expressed as τ · δq −

nb 

Mi (iaGi + gieˆ03 ) · (iGi δq) −

i=1

nb 

(iIiGi iαi + iωi × iIiGi iωi ) · (ii δq) = 0,

i=1

(5.50)

∀δq ∈ Rn , or τ · δq −

nb ' (  Mi iTGi iaGi + Mi giTGi ieˆ03 + iTi (iIiGi iαi + iωi × iIiGi iωi ) · δq = 0 i=1

(5.51)

∀δq ∈ Rn . In matrix form we have % &  nb    Mi iaGi + Mi gieˆ03 i T i T Gi i τ− · δq = 0, G i Gi i Ii αi + iωi × iIi i iωi i=1

(5.52)

∀δq ∈ Rn , which implies τ=

nb  

i



Gi i T

i

T



Mi iaGi + Mi gieˆ03 I αi + ii iIi i iωi

i Gi i i

i=1

G

 .

(5.53)

Noting that ωi = ii q, ˙

aGi = iGi q¨ + (i˙ Gi + ii iGi )q, ˙

i

i

and

i

˙ i q, αi = ii q¨ + i ˙

(5.54)

we have τ=

%n b 

& (Mi Gi Gi + i I i ) q¨ i

T

i

i

Ti

Gi i i

i=1

%

+

nb   i=1

˙ i +  I i ) Mi Gi ( ˙ Gi + i Gi ) + i ( I  i

T

i

i

i

i

T

i Gi i i

i

i Gi i i i

& 

nb  Mi iTGi ieˆ03 . q˙ + g i=1

(5.55)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

111

Algorithm 3 Second-order method for integrating the equations of motion 1:

q0 = qo {initialization}

2:

q˙0 = q˙o {initialization}

3:

for i = 0 to ns − 1 do

4:

q¨i = Mi−1 (τi − bi − gi )

5:

q˙i+1 = q˙i + q¨i t

6:

qi+1 = qi + q˙i t + 12 q¨i t 2

7:

end for

Defining the symmetric positive definite mass matrix, M (q), the centrifugal and Coriolis force vector, b(q, q), ˙ and the gravity force vector, g(q),

M (q) 

nb 

(Mi iTGi iGi + iTi iIiGi ii ),

(5.56)

i=1

%n & b    i T i˙ i i T i Gi i ˙ i i i Gi i Mi Gi ( Gi + i Gi ) + i ( Ii i + i Ii i ) q, b(q, q) ˙  ˙

(5.57)

i=1 nb  Mi iTGi ieˆ03 , g(q)  g

(5.58)

i=1

we have τ = M (q)q¨ + b(q, q) ˙ + g(q).

(5.59)

Numerical Integration We can numerically integrate (5.59) using a second-order method. Solving for the generalized accelerations, we have q¨ = M −1 (τ − b − g).

(5.60)

The second-order method for integrating this system can be summarized as shown in Algorithm 3.

Example: We consider a serial chain robot and parameterize the system using four generalized coordinates as shown in Figure 5.2. The centers of mass of each link can be

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

112

Zeroth-Order Variational Principles

Figure 5.2 A 4 degree-of-freedom serial chain robot parameterized using four generalized

coordinates.

computed in a straightforward fashion:   l1 cos(q ) 1 rG1 = 2l1 , sin(q1 ) 2   l1 cos(q1 ) + l22 cos(q1 + q2 ) rG2 = , l1 sin(q1 ) + l22 sin(q1 + q2 )   l1 cos(q1 ) + l2 cos(q1 + q2 ) + l23 cos(q1 + q2 + q3 ) rG 3 = , l1 sin(q1 ) + l2 sin(q1 + q2 ) + l23 sin(q1 + q2 + q3 )  l1 cos(q1 ) + l2 cos(q1 + q2 ) + l3 cos(q1 + q2 + q3 ) rG4 = l1 sin(q1 ) + l2 sin(q1 + q2 ) + l3 sin(q1 + q2 + q3 ) + l24 cos(q1 + q2 + q3 + q4 ) ··· + l24 sin(q1 + q2 + q3 + q4 )

(5.61) (5.62) (5.63) (5.64)  .

(5.65)

Similarly, the angular velocities of each link can be computed easily: ω1 = q˙1 ,

(5.66)

ω2 = q˙1 + q˙2 ,

(5.67)

ω3 = q˙1 + q˙2 + q˙3 ,

(5.68)

ω4 = q˙1 + q˙2 + q˙3 + q˙4 .

(5.69)

The translational Jacobians are computed as Gi =

∂rGi , ∂q

for i = 1, . . . , 3,

(5.70)

for i = 1, . . . , 3.

(5.71)

and the angular velocity Jacobians as i =

∂ωi , ∂ q˙

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

113

Figure 5.3 (Top) Animation frames from the simulation of a serial chain robot. (Bottom) Time history of the robot generalized coordinates (q1 , q2 , q3 , q4 ).

The dynamical terms are

M (q) =

4 

(Mi TGi Gi + Ti IiGi i ),

(5.72)

Mi TGi ˙ Gi q, ˙

(5.73)

i=1

b(q, q) ˙ =

4  i=1

4  g(q) = g Mi TGi eˆ2 ,

(5.74)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

114

Zeroth-Order Variational Principles

where we take the rotational inertias to be Mi li2 Mi ri2 + . (5.75) 4 12 The simulation results are displayed in Figure 5.3. The following values were used for the constants: IiGi =

r1 = 0.15,

r2 = 0.125,

r3 = 0.1,

r4 = 0.0875,

l1 = 0.7,

l2 = 0.6,

l3 = 0.5,

l4 = 0.4,

M1 = 2.5,

M2 = 1.5,

M3 = 1.0,

M4 = 0.75.

(5.76)

The initial conditions used were qi = −π /24 and q˙i = 0.

5.2.5

Auxiliary Constraints Holonomic Constraints The standard taxonomy of multibody systems by kinematic topology consists of branching or tree-like structures and graph or closed-loop structures. In the case of branching structures a set of independent generalized coordinates is chosen. The kinematic constraints between the bodies are implied by the choice of these generalized coordinates, as we saw in Section 5.2.4. Since these generalized coordinates are independent (unconstrained), we will refer to branching structures as unconstrained with respect to configuration space. Analysis of graph or closed-loop structures typically involves breaking the loop(s) and deriving the dynamics of the resulting branching structures. The last step involves the imposition of a set of holonomic constraint equations, φ(q) = 0, to enforce the loop closures. A closed-loop topology is depicted in Figure 5.4 (left), with the nodes representing the bodies and the edges representing the joints. Closed-loop structures represent a subset of the larger class of holonomically constrained multibody systems. Such systems involve holonomic constraints in the form of general algebraic dependencies between generalized coordinates, as depicted in Figure 5.4 (right). Again, a set of holonomic constraint equations is imposed in conjunction with the unconstrained equations of motion. Since the systems of Figure 5.4 involve explicit, or auxilliary, constraints between the generalized coordinates (in addition to the implicit constraints between the bodies suggested by the choice of generalized coordinates), we will refer to these structures as constrained with respect to configuration space. In Section 3.4 we considered the number of degrees of freedom of holonomically constrained systems. The number of degrees of freedom, p, is given by p = n − m,

(5.77)

where n is the number of generalized coordinates and m is the number of independent constraint equations. The constrained systems described thus far exclusively involve holonomic constraints. Nonholonomic systems, which involve nonintegrable constraints on the generalized velocities, will be addressed in the subsequent chapters. They can be handled with

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

115

Figure 5.4 Constrained structures involving closed loops (Left) and general algebraic dependencies (Right). The dynamics of both systems can be derived from the unconstrained system dynamics through the imposition of a set of holonomic constraint equations, φ(q) = 0.

higher-order variational principles (Flannery 2005) and methods like Jourdain’s Principle, Kane’s method, Gauss’s Principle, or the Gibbs-Appell method. Jourdain’s Principle and Gauss’s Principle deal with the constraints explicitly, while Kane’s method and the Gibbs-Appell method deal with the costraints implicitly by defining a set of independent quasi-velocities (generalized speeds) and/or quasi-accelerations. We now consider the general case of auxiliary holonomic constraints. Given the multibody system τ = M (q)q¨ + b(q, q) ˙ + g(q),

(5.78)

subject to the holonomic constraints φ(q) = 0,

(5.79)

we begin by first expressing the zeroth-order variational equation associated with d’Alembert’s Principle: τC · δq + (τ − Mq − b − g) · δq = 0,

(5.80)

where τC is the vector of generalized constraint forces. The virtual displacements, δq, refer to all displacement variations which satisfy the constraints, while time is fixed. With δt = 0 the variation of the constraint equation yields δφ =

∂φ δq = δq = 0, ∂q

(5.81)

which implies that δq ∈ ker(), where  = ∂φ is the constraint matrix. Under this con∂q dition, (5.80) can be restricted to constraint-consistent virtual displacements: τC · δq + (τ − Mq − b − g) · δq = 0 ∀δq ∈ ker().

(5.82)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

116

Zeroth-Order Variational Principles

Figure 5.5 The configuration space constrained-motion manifold, Q p . All constraint-consistent virtual variations, δq, lie in the tangent space of Q p and are orthogonal to the constraint forces.

We have τC ⊥δq ∀δq ∈ ker().

(5.83)

The ker() represents the tangent space of the constrained-motion manifold, Q p , at a point, q, in configuration space. The constraint-consistent virtual displacements, δq, lie in this tangent space, and the generalized constraint forces, τC , are orthogonal to it. This is illustrated in Figure 5.5. Based on this, the following is implied: τC ∈ ker()⊥ = im(T ).

(5.84)

Thus, the generalized constraint force, τC , can be represented as a linear combination of the columns of T . That is, τC = T λ, where λ is a vector of unknown Lagrange multipliers. The term τC · δq vanishes from (5.82) and we have the orthogonality relation (Mq + b + g − τ) · δq = 0 ∀δq ∈ ker().

(5.85)

The constrained multibody equations of motion are then τ = M q¨ + b + g − T λ,

(5.86)

subject to φ(q) = 0

⇒

φ˙ = 0, φ¨ = 0.

(5.87)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

117

Solution of the Constrained Dynamics Problem We can arrive at an explicit solution of the constrained dynamics problem, M q¨ = τ − b − g + T λ ˙ q. q¨ = − ˙

(5.88) (5.89)

It is useful to define the mC × mC constraint space mass matrix, which reflects the system inertia projected at the constraint: H  (M −1 T )−1 .

(5.90)

We now express the mass-weighted (right) inverse of : ¯ = M −1 T (M −1 T )−1 , 

(5.91)

¯ = 1 and, equivalently,  ¯ T T = 1. Defining the n × n constraint null space where  ¯ matrix,  1 − , we can solve the system. This yields ¯ ˙ q˙ + M −1 (τ − b − g) q¨ = − ˙ q. ¯ T (τ − b − g) − H ˙ λ = −

(5.92) (5.93)

It is noted that  and satisfy the condition  = 0 and, equivalently, T T = 0. Furthermore, if we form the projection matrix P T = P which projects any vector in Rn onto the null space of , we have P T = W W T = 1 − T +T = 1 − T (T )−1 ,

(5.94)

where W spans the null space of , and + = T (T )−1 is the pseudoinverse (right inverse) of . The expression for P T in (5.94) has a similar form as the expression ¯ T = 1 − T (M −1 T )−1 M −1 .

T = 1 − T 

(5.95)

Consequently P T = W W T can be regarded as a kinematic constraint null space projection matrix and T can be regarded as a mass-weighted constraint null space projection matrix. The physical and geometric meaning of  and will be discussed further in Section 8.1.3.

Numerical Integration In the previous section we solved the constrained dynamical system for the generalized accelerations and the Lagrange multipliers. In practice the integration of the forward dynamics would also require constraint stabilization to mitigate drift in the constraints. Baumgarte stabilization (Baumgarte 1972) involves replacing our original acceleration constraint equation with a linear combination of acceleration, velocity, and position constraint terms: φ¨ + β φ˙ + αφ = 0

(5.96)

˙ q˙ + βq˙ + αφ = 0, q¨ + 

(5.97)

or

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

118

Zeroth-Order Variational Principles

Algorithm 4 Second-order method for integrating the zeroth-order holonomically constrained equations of motion 1:

q0 = qo {initialization}

2:

q˙0 = q˙o {initialization}

for i = 0 to ns − 1 do ¯ i ¯ i (αφi + βi q˙i ) ˙ i q˙i + i M −1 (τi − bi − gi ) −  4: q¨i = − i ¯ Ti (τi − bi − gi ) − Hi  ˙ i q˙i − Hi (αφi + βi q˙i ) 5: λi = − 3:

6:

q˙i+1 = q˙i + q¨i t

7:

qi+1 = qi + q˙i t + 12 q¨i t 2

8:

end for

where α and β are constant parameters chosen to drive the first- and zeroth-order derivatives of the holonomic constraint equations to zero, thereby compensating for position and velocity drift in the constraints. The constraint stabilized equations of motion are then      q¨ τ −b−g M −T = , (5.98) ˙ q˙ + βq˙ + αφ − 0 λ  and the solution of this system is ¯ ¯ ˙ q˙ + M −1 (τ − b − g) − (αφ + βq) ˙ q¨ = − T ˙ ¯ ˙ λ = − (τ − b − g) − Hq˙ − H(αφ + βq).

(5.99) (5.100)

The term −H(αφ + βq) ˙ in the expression for λ can be physically interpreted as a corrective constraint force term used to compensate for any drift in the constraints. This is analogous to a proportional-derivative (PD) control law in a feedback system. A second-order method for integrating this system can be summarized as shown in Algorithm 4.

Example: A Stewart platform parallel mechanism can be described by a set of 24 generalized coordinates, as shown in Figure 5.6. The constraint equations associated with the loop closures are given by rli = r pi

for i = 1, · · · , 6,

(5.101)

where rli is the terminal point of the ith strut subsystem which connects to r pi , the ith position on the platform subsystem. In vector form we have ⎞ ⎛ rl1 − r p1 ⎟ ⎜ .. (5.102) φ=⎝ ⎠ = 0. . rl6 − r p6

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

119

Figure 5.6 (Left) Stewart platform actuated by six prismatic struts (remaining joints are passive).

(Right) The closed-loop mechanism is cut at various locations to create serial chains described by the set of generalized coordinates, q1 , . . . , q24 .

Taking the derivative yields ⎞ r˙l1 − r˙p1 ⎟ ⎜ .. φ˙ = ⎝ ⎠ = q˙ = 0, . ⎛

(5.103)

r˙l6 − r˙p6 where ⎛

q˙3i−2

⎛

⎞ q˙19 ⎜ . ⎟ ⎟ r˙pi =  pi ⎜ ⎝ .. ⎠ ,

⎞

⎜ ⎟ r˙li = li ⎝ q˙3i−1 ⎠

and,

q˙3i

(5.104)

q˙24 for i = 1, · · · , 6.

The terms, li and  pi , are the corresponding Jacobians of rli and r pi , respectively. So, ⎞⎛ ⎞ ⎛ q˙1 l1 . . . 0 − p1 ⎜ .. .. ⎟ ⎜ .. ⎟ = 0, .. (5.105) q˙ = ⎝ ... . . . ⎠⎝ . ⎠ ...

 l6

− p6

 l1 ⎜ .. =⎝ .

... .. . ...

0 .. .

0 where

⎛

0

 l6

q˙24 ⎞ − p1 .. ⎟ . . ⎠

(5.106)

− p6

The constraint forces, λ, are shown in Figure 5.7.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

120

Zeroth-Order Variational Principles

Figure 5.7 Constraint forces associated with loop closures. Lagrange multipliers, λ, represent constraint forces at various locations between the serial chains to enforce the constraints associated with the loop closures.

The unconstrained equations of motion for the six strut serial subsystems are ⎛ ⎞ ⎛ ⎞ q¨1 0 ⎝ 0 ⎠ = Ml1 ⎝ q¨2 ⎠ + bl1 (q1 , . . . , q˙3 ) + gl1 (q1 , . . . , q3 ), (5.107) q¨3 τ1 .. . ⎛ ⎞ ⎛ ⎞ 0 q¨16 ⎝ 0 ⎠ = Ml6 ⎝ q¨17 ⎠ + bl6 (q16 , . . . , q˙18 ) + gl6 (q16 , . . . , q18 ), (5.108) τ6 q¨18 and for the platform subsystem we have ⎛ ⎞ q¨19 ⎜ ⎟ 0 = M p ⎝ ... ⎠ + b p (q19 , . . . , q˙24 ) + g p (q19 , . . . , q24 ).

(5.109)

q¨24 The entire unconstrained system is described by ⎛ ⎞ 0 ⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎜τ ⎟ Ml1 0 ··· 0 q¨1 bl1 gl1 ⎜ 1⎟ ⎜ . ⎟ ⎜ .. ⎟ ⎜ .. ⎜ ⎜ ⎟ ⎟ . . ⎟ q ¨ ⎜ . ⎟ ⎜ 0 ⎟ 2 . . ⎟⎜ ⎟ ⎜ .. ⎟ ⎜ .. ⎟ ⎜ . ⎟=⎜ .. ⎟ + ⎜ ⎟ + ⎜ ⎟ , ⎜ ⎟ ⎜ .. ⎟⎜ ⎝ . ⎠ ⎝ bl ⎠ ⎝ gl ⎠ ⎜0⎟ ⎝ . 6 6 Ml6 0 ⎠ ⎜ ⎟ ⎝ τ6 ⎠ b g q ¨ 24 p p 0 ··· 0 Mp 0

(5.110)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

121

and the constrained system is ⎛

⎞ 0 ⎛ ⎜τ ⎟ Ml1 ⎜ 1⎟ ⎜ . ⎟ ⎜ ⎜ . ⎟ ⎜ 0 ⎜ . ⎟=⎜ ⎜ ⎟ ⎜ .. ⎜0⎟ ⎝ . ⎜ ⎟ ⎝ τ6 ⎠ 0 0 ⎛

···

0 .. .

Ml6 0

···

Tl ⎜ .1 ⎜ .. +⎜ ⎜ ⎝ 0 −Tp1

... .. . ... ···

⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 0 q¨1 bl1 gl1 .. ⎟ ⎜ ⎜ ⎜ ⎟ ⎟ . . ⎟ q ¨ ⎟ . ⎟ ⎜ 2 ⎟ ⎜ .. ⎟ ⎜ .. ⎟ .. ⎟ + ⎜ ⎟ + ⎜ ⎟ ⎟⎜ ⎝ . ⎠ ⎝ bl ⎠ ⎝ gl ⎠ 6 6 0 ⎠ bp gp q¨24 Mp ⎞ 0 ⎛ ⎞ λ1 .. ⎟ ⎟ . ⎟ . ⎟⎜ ⎝ .. ⎠ , T ⎟  l6 ⎠ λ18 −Tp6

(5.111)

where the dimensions of the terms are, M ∈ Rn×n ,

b, g, τ ∈ Rn ,

 ∈ Rm×n ,

λ ∈ Rm ,

(5.112)

and n = 24 and m = 18. The constrained system has p = n − m = 6 degrees of freedom. Noting that the generalized constraint force τC is given by τC = T λ,

(5.113)

the virtual work of the constraint forces is given by τC · δq = τCT δq = λT δq.

(5.114)

So, τC · δq = 0

(5.115)

∀δq ∈ δQ p , where δQ p = ker(). Furthermore, we note that ⎞ δrl1 − δr p1 ⎟ ⎜ .. δq = ⎝ ⎠. . ⎛

(5.116)

δrl6 − δr p6 So, ⎞T ⎛ δrl1 λ1 ⎜ .. ⎟ ⎜ T τC · δq = λ δq = ⎝ . ⎠ ⎝ ⎛

λ18

⎞ − δr p1 ⎟ .. ⎠. .

(5.117)

δrl6 − δr p6

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

122

Zeroth-Order Variational Principles

Thus, the virtual work of the constraint forces can also be expressed by ⎛ ⎞ δrl1 λ1 ⎜ ⎜ .. ⎟ ⎜ ⎝ . ⎠·⎜ ⎝ λ18 δrl6 ⎛

⎞ − δr p1 ⎟ .. ⎟ ⎟ = 0, . ⎠ − δr p6

(5.118)

∀δrli , δr pi |δrli = δr pi , i = 1, . . . , 6.

Example: A parallel mechanism is depicted in Figure 5.8. The constraint equations describe the loop closures and are given by ⎛

r l1 − r p 1

⎞

⎟ ⎜ φ(q) = ⎝ rl2 − r p2 ⎠ ,

(5.119)

rl3 − r p3 where rli is the terminal point of the ith elbow chain subsystem which connects to r pi , the ith position on the platform subsystem. Taking the derivative yields ⎛

r˙l1 − r˙p1

⎞

⎟ ⎜ φ˙ = ⎝ r˙l2 − r˙p2 ⎠ = q˙ = 0,

(5.120)

r˙l3 − r˙p3

Figure 5.8 Parallel mechanism consisting of serial chains with loop closures. The three elbow

joints are actuated, while the remaining joints are passive.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

where  r˙li = li

q˙2i−1 q˙2i

⎛

 and,

⎞ q˙7 r˙pi =  pi ⎝ q˙8 ⎠ , q˙9

123

(5.121)

for i = 1, . . . , 3. The terms, li and  pi are the corresponding Jacobians of rli and r pi , respectively. So, ⎞ ⎛ q˙ ⎞ ⎛ 1  l1 0 0 − p1 ⎜ .. ⎟ ⎠ ⎝ (5.122) q˙ = 0 l2 0 − p2 ⎝ . ⎠ = 0, 0 0 l3 − p3 q˙9 where

⎛

 l1 ⎝ = 0 0

0  l2 0

0 0  l3

For the three elbow link chains we compute  −li1 sin(q2i−1 ) − li2 sin(q2i−1 + q2i )  li = li1 cos(q2i−1 ) + li2 cos(q2i−1 + q2i )

⎞ − p1 − p2 ⎠ . − p3

(5.123)

−li2 sin(q2i−1 + q2i )



li2 cos(q2i−1 + q2i )

,

(5.124)

for i = 1, . . . , 3, and for the platform we compute  −1 0  p1 = 0 −1  −1 0  p2 = 0 −1  −1 0  p3 = 0 −1

−1.08 sin(0.47 − q9 ) −1.08 cos(0.47 − q9 ) 1.08 sin(1.62 + q9 ) −1.08 cos(1.62 + q9 ) 1.08 sin(3.72 + q9 ) −1.08 cos(3.72 + q9 )

 ,

(5.125)

,

(5.126)

,

(5.127)

 

for i = 1, . . . , 3. For the six elbow link centers of mass (one proximal and one distal link for each chain) we compute  l  − 2i1 sin(q2i−1 ) 0 , Gli1 = li1 cos(q2i−1 ) 0 (5.128) 2 for i = 1, . . . , 3, and

 Gli2 =

−li1 sin(q2i−1 ) − li1 cos(q2i−1 ) +

li2 2

li2 2

sin(q2i−1 + q2i )

cos(q2i−1 + q2i )

− l2i2 sin(q2i−1 + q2i ) li2 2

cos(q2i−1 + q2i )

 ,

(5.129)

for i = 1, . . . , 3,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

124

Zeroth-Order Variational Principles

The terms, Gli j and li j , are the center of mass Jacobian and link length, respectively, of the jth link of the ith elbow chain subsystem. The unconstrained equations of motion for the three elbow chain subsystems are     q¨2i−1 0 = Mli + bli (q2i−1 , . . . , q˙2i ) + gl1 (q2i−1 , q2i ), q¨2i τi (5.130) for i = 1, . . . , 3, where, taking the rotational inertia of the individual links to be zero, we have Mli =

2 

Mli j TGl Gli j ,

(5.131)

ij

j=1

⎛ ⎞ 2  bli = ⎝ Mli j TGl ˙ Gli j ⎠ q, ˙

(5.132)

ij

j=1

gli = g

2 

Mli j TGl eˆ2 ,

(5.133)

ij

j=1

for i = 1, . . . , 3. The term, Mli j , is the mass of the jth link of the ith elbow chain subsystem. The terms are computed as 1 2 [l M + li12 (Mli1 + 4Mli2 ) + 4li1 li2 Mli2 cos(q2i )] 4 i2 li2 Mli = 1 l M [l + 2li1 cos(q2i )] 4 i2 li2 i2  1 l M [l + 2li1 cos(q2i )] 4 i2 li2 i2 ··· , (5.134) 1 2 l M 4 i2 li2   − 12 li1 li2 Mli2 sin(q2i )q˙2i (2q˙2i−1 + q˙2i ) bli = , (5.135) 1 l l M sin(q2i )q˙22i−1 2 i1 i2 li2   1 g[li1 (Mli1 + 2Mli2 ) cos(q2i−1 ) + li2 Mli2 cos(q2i−1 + q2i )] 2 gli = , (5.136) 1 gl M cos(q2i−1 + q2i ) 2 i2 li2 for i = 1, . . . , 3. For the platform subsystem we have ⎛ ⎞ q¨7 0 = M p ⎝ q¨8 ⎠ + b p (q7 , . . . , q˙9 ) + g p (q7 , q8 , q9 ), q¨9

(5.137)

where M p = M p TG p G p + IpG p Tp  p ,   ˙ p q, ˙ b p = M p TG p ˙ G p + IpG p Tp  g p = gM p TG p eˆ2 .

(5.138) (5.139) (5.140)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.2 D’Alembert’s Principle of Virtual Work

125

The Jacobians are simply G p = 12×2 and  p = 1. The dynamical terms are then computed as ⎛ ⎞ Mp 0 0 (5.141) Mp = ⎝ 0 Mp 0 ⎠ , 0 0 IpG p b p = 0, ⎛

(5.142)

⎞

0 g p = ⎝ M pg ⎠ . 0

(5.143)

The entire unconstrained system is composed as ⎛ ⎞ 0 ⎜τ ⎟ ⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎜ 1⎟ Ml1 q¨1 0 0 0 gl1 bl1 ⎜0⎟ ⎜ ⎟ ⎜ .. ⎟ ⎜ q¨2 ⎟ ⎜ ⎟ ⎜ ⎟ b gl2 ⎟ ⎜ ⎟ ⎜ 0 Ml2 ⎜ ⎟ l2 ⎟ 0 . ⎟ +⎜ , ⎜ τ2 ⎟ = ⎜ ⎟⎜ . ⎟ + ⎜ ⎝ ⎝ ⎠ . ⎜ ⎟ ⎝ 0 bl3 gl3 ⎠ 0 ⎠⎝ . ⎠ 0 Ml3 ⎜0⎟ ⎜ ⎟ bp gp 0 0 0 Mp q¨9 ⎝ τ3 ⎠ 0

(5.144)

and the constrained system is ⎛

⎞ 0 ⎜τ ⎟ ⎛ ⎜ 1⎟ Ml1 ⎜0⎟ ⎜ ⎟ ⎜ ⎜ ⎟ ⎜ 0 ⎜ τ2 ⎟ = ⎜ ⎜ ⎟ ⎝ 0 ⎜0⎟ ⎜ ⎟ 0 ⎝ τ3 ⎠ 0 ⎛

0

0

Ml2 0 0

0 Ml3 0

Tl ⎜ 01 ⎜ +⎜ ⎝ 0 −Tp1

0 Tl2 0 −Tp2

⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ q¨1 0 bl1 gl1 .. ⎟ ⎜ q¨2 ⎟ ⎜ ⎟ ⎜ ⎟ bl2 ⎟ ⎜ gl2 ⎟ ⎜ ⎟ . ⎟ ⎟⎜ . ⎟ + ⎜ ⎝ bl ⎠ + ⎝ gl ⎠ . ⎝ ⎠ ⎠ 3 3 0 . b g p p Mp q¨9 ⎞ ⎛ ⎞ 0 λ1 ⎟ 0 ⎟⎜ . ⎟ ⎟⎝ . ⎠, Tl3 ⎠ . λ6 −Tp3

(5.145)

where the dimensions of the terms are M ∈ Rn×n ,

b, g, τ ∈ Rn ,

 ∈ Rm×n ,

λ ∈ Rm ,

(5.146)

and n = 9 and m = 6. The constrained system has p = n − m = 3 degrees of freedom. Results from simulation of the system under gravity are shown in Figures 5.9 and 5.10. All geometric and inertial constants were chosen to be 1: li j = 1, Mi j = 1, M p = 1, IpG p = 1, for i = 1, . . . , 3, and, j = 1, 2.

(5.147)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

126

Zeroth-Order Variational Principles

Figure 5.9 (Top) Animation frames from the simulation of the parallel mechanism falling under

gravity. (Bottom) Time history of the motion of the platform (x, y, θ ).

The initial conditions used were

qo = π /4 π /6 11π /12 π /6 −5π /12

T q˙o = 0 0 0 0 0 0 0 0 0 .

π /6

0

2.165

0

T

(5.148) (5.149)

It can be verified that these satisfy the constraints.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

127

5.2 D’Alembert’s Principle of Virtual Work

Figure 5.10 Lagrange multipliers (constraint forces) for the parallel mechanism falling under

gravity.

5.2.6

Generalized Constrained Equation of Motion Given the explicit solution of the constrained dynamics problem from Section 5.2.5, we wish to express an alternate form of the constrained dynamical equations of motion. We begin by recalling (5.93): ˙ q. ¯ T (τ − b − g) − H ˙ λ = −

(5.150)

Substituting (5.150) into (5.86) yields ˙ q˙ + (1 − T  ¯ T )τ + T  ¯ T (b + g). M q¨ + b + g = −T H

(5.151)

We now define the mC × 1 vector of centrifugal and Coriolis forces projected at the constraint, ˙ q, ¯ T b − H ˙ α

(5.152)

and the mC × 1 vector of gravity forces projected at the constraint, ¯ T g. ρ

(5.153)

Thus, we have the concise expression, which we will refer to as the generalized constrained equation of motion (De Sapio, Khatib, and Delp 2006):

T τ = M q¨ + b + g − T (α + ρ).

(5.154)

An alternative means of deriving this equation involves directly mapping the configuration space equation (5.86) into the constraint null space using T ,

T τ = T M q¨ + T b + T g − T T λ.

(5.155)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

128

Zeroth-Order Variational Principles

Noting that T T = 0 and manipulating, we have ¯ T M q¨ − T  ¯ T b − T  ¯ Tg

T τ = M q¨ + b + g − T  ˙ q. ˙ = M q¨ + b + g − T Hq¨ − T (p + α) − T H

(5.156)

˙ q˙ = −q, Substituting in our constraint condition,  ¨ yields

T τ = M q¨ + b + g − T (p + α).

5.3

(5.157)

Hamilton’s Principle of Least Action An alternate way of arriving at the the equations of motion is through a least action principle. The fundamental theme associated with least action principles is that the evolution of a dynamical system can be revealed by examining the stationary condition for an appropriately defined action integral. While least action can refer to a general family of variational principles, perhaps the most significant among these is Hamilton’s Principle of Least Action. This principle states that the path, q(t ), of a system in configuration space over an interval, [to, t f ], is such that the action is stationary under all path variations that vanish at the endpoints, q(to ) and q(t f ). It is noted that this does not strictly imply a minimization of the action, as the name of the principle suggests, but rather an extremization of the action. p r i n c i p l e 5.2 For scleronomic systems (no explicit time dependence) the path of a system in configuration space during an interval, [to, t f ], is such that the action

t f L(q, q)dt ˙

I=

(5.158)

to

is stationary under all path variations. The scalar term, L, is the Lagrangian, defined as L  T − V,

(5.159)

where T and V are the system kinetic and potential energies, respectively. Furthermore δI = 0 ∀δ|δq(to ) = δq(t f ) = 0.

(5.160)

This is known as Hamilton’s Principle of Least Action. For forced systems the Principle of Least Action is modified such that the variation in the action is given by

t f

t f L dt +

δI = δ to

τ · δq dt.

(5.161)

to

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

129

5.3 Hamilton’s Principle of Least Action

5.3.1

Euler-Lagrange Equations It is straightforward to apply calculus of variations to this problem (Goldstein, Poole, and Safko 2002). We can express the first term in (5.161) as

t f 

t f L dt =

δ to

to



t f

t f ∂L ∂L ∂L ∂L d(δq) · δq + · δ q˙ dt = · δq dt + · dt. ∂q ∂ q˙ ∂q ∂ q˙ dt to

to

(5.162) Noting that ∂L d(δq) d · = ∂ q˙ dt dt



 ∂L d ∂L · δq − · δq, ∂ q˙ dt ∂ q˙

(5.163)

we have

t f  to

  

t f ∂L d ∂L ∂L d(δq) · δq + · dt = · δq dt ∂q ∂ q˙ dt dt ∂ q˙ to

t f  + to

 ∂L d ∂L · δq − · δq dt. ∂q dt ∂ q˙

(5.164)

Since the variations, δq, vanish at the endpoints,

t f

d dt



to

)t f  ) ∂L ∂L · δq dt = · δq)) = 0. ∂ q˙ ∂ q˙ to

(5.165)

Thus,

t f 

t f to

 · δq dt

(5.166)

 ∂L d ∂L − + τ · δq dt. ∂q dt ∂ q˙

(5.167)

L dt =

δ

∂L d ∂L − ∂q dt ∂ q˙

to

and

t f  δI = to

The condition δI = 0 ∀δq,

(5.168)

implies the following Euler-Lagrange equations: τ=

d ∂L ∂L − . dt ∂ q˙ ∂q

(5.169)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

130

Zeroth-Order Variational Principles

We can express the Euler-Lagrange equations as τ=

5.3.2

∂T ∂V d ∂T − + . dt ∂ q˙ ∂q ∂q

(5.170)

A Single Particle For a single point mass with a discrete set of n f external forces, { f1 , . . . , fn f }, acting on it, the Euler-Lagrange equations are nf 

fi =

i=1

∂T ∂V d ∂T − + . dt ∂ r˙ ∂r ∂r

(5.171)

The kinetic energy, T , is given by T =

1 1 Mv T v = M r˙T r˙. 2 2

(5.172)

So, ∂T = M r˙ ∂ r˙

(5.173)

d ∂T = M r¨ = Ma. dt ∂ r˙

(5.174)

∂T = 0. ∂r

(5.175)

V = M r, geˆ3  = MgrT eˆ3 .

(5.176)

∂V = Mgeˆ3 . ∂r

(5.177)

fi = Ma + Mgeˆ3 .

(5.178)

and

We note that

The potential energy, V , is given by

So,

Thus, nf  i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.3 Hamilton’s Principle of Least Action

5.3.3

131

A Single Rigid Body For a single rigid body with a discrete set of n f external forces, { f1 , . . . , fn f }, and nϕ external moments, {ϕ1 , . . . , ϕnϕ }, acting on it, the Euler-Lagrange equations are ⎞ ⎛ nf fi ⎟ ⎜ i=1 ⎟ = d ∂T − ∂T + ∂V , ⎜n (5.179) nϕ ⎠ dt ∂ x˙ ⎝ f ∂x ∂x −→ × f + dGP ϕ i j i i=1

i=1

where

 x=

rG



 x˙ =

and

θ

vG

 .

ω

(5.180)

The kinetic energy, T , is given by T =

1 (Mv TG v G + ωT I G ω). 2

So, ∂T = ∂ x˙ and d ∂T = dt ∂ x˙





Mv G I Gω

(5.181)

 (5.182) 

MaG I Gα + ω × I Gω

.

(5.183)

The potential energy, V , is given by V = M r, geˆ3  = MgrT eˆ3 . So, ∂V = ∂x Thus,

⎛ ⎜ ⎜n ⎝ f i=1

nf



Mgeˆ3 0

⎞ fi

i=1 −→ × f + dGP i i

nϕ

ϕj

⎟ ⎟= ⎠



(5.184)

 .

MaG + Mgeˆ3 I Gα + ω × I Gω

(5.185)

 .

(5.186)

i=1

The Kinetic Energy Ellipsoid Rotational kinetic energy is given by 1 T G ω I ω. (5.187) 2 We will assume this expression is represented in the base frame for convenience. Since I G is symmetric positive definite, it has positive eigenvalues and an orthogonal T =

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

132

Zeroth-Order Variational Principles

eigenbasis (principal axes), E. Therefore, T =

1 1 O T O GO ω I ω = E ωT OEQT OI G OEQE ω, 2 2

(5.188)

where the columns of OEQ are the eigenvectors, {v 1 , v 2 , v 3 }, of OI G , ⎛

↑ O ⎝ Q = v 1 E ↓

↑ v2 ↓

⎞ ↑ v 3 ⎠. ↓

(5.189)

We then have QT OI G OEQ = E I G ,

O E

(5.190)

where E I G is a diagonal matrix of eigenvalues, {λ1 , λ2 , λ3 }, of OI G : ⎛

λ1 E G I =⎝ 0 0

0 λ2 0

⎞ 0 0 ⎠. λ3

(5.191)

In terms of the eigenbasis, T =

1 E T E GE ω I ω. 2

(5.192)

For constant values of the kinetic energy T , this represents an ellipsoid expressed with respect to the principal axes. In scalar form, T =

 1 E 2 λ1 ω1 + λ2 E ω22 + λ3 E ω32 . 2

(5.193)

The semiaxes of the ellipsoid in frame E are then  ai = 2T /λi .

(5.194)

Example: We consider the cone addressed in Section 4.1.2. The inertia tensor about the center of mass is ⎞ ⎛ 3 M (h2 + 4R2 ) 0 0 80 ⎟ ⎜ 3 (5.195) IG = ⎝ M (h2 + 4R2 ) 0 ⎠. 0 80 0

0

3 2 R 10

Specifying M = 1, h = 1, and R = .75, the eigenvalues are λ1 = 0.684, λ2 = 0.684, and λ3 = 0.169. The ellipsoid associated with a constant kinetic energy, T = 1, has semiaxes a1 = 1.71, a2 = 1.71, and a3 = 3.443, aligned with the cone axes depicted in Figure 4.3.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.3 Hamilton’s Principle of Least Action

5.3.4

133

A System of Particles For a system of particles with generalized forces, τ, acting on the system the EulerLagrange equations are τ=

∂T ∂V d ∂T − + . dt ∂ q˙ ∂q ∂q

(5.196)

The kinetic energy, T , is given by 1 1 Mi v Ti v i = q˙T T = 2 i=1 2 np

We note that

So,

 np 

 Mi i i q. ˙ T

(5.197)

i=1

 np   ∂T T = Mi i i q. ˙ ∂ q˙ i=1

(5.198)

  np  d ∂T T Mi i i q. ¨ = dt ∂ q˙ i=1

(5.199)

The potential energy, V , is given by V =

np 

Mi ri , geˆ3  = g

i=1

np 

Mi riT eˆ3 .

(5.200)

i=1

So  ∂V =g Mi ∂q i=1 np



∂ri ∂q

T eˆ3 = g

np 

Mi Ti eˆ3

(5.201)

i=1

and τ=

np 

Ti (Mi ai + Mi geˆ3 ).

(5.202)

i=1

Thus,

 np  nq   ∂T ∂V d ∂T T − + = τ= Mi i i q¨ + g Mi Ti eˆ3 . dt ∂ q˙ ∂q ∂q i=1 i=1

(5.203)

For a system of particles, we had previously defined M (q) 

np 

Mi Ti i

(5.204)

i=1 np

 g(q)  g Mi Ti eˆ3 .

(5.205)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

134

Zeroth-Order Variational Principles

So, τ = M (q)q¨ + g(q).

5.3.5

(5.206)

A System of Rigid Bodies For a system of rigid bodies with generalized forces, τ, acting on the system, the EulerLagrange equations are τ=

∂T ∂V d ∂T − + . dt ∂ q˙ ∂q ∂q

(5.207)

The kinetic energy, T , is given by nb   1 Mi iv TGi iv Gi + iωiT iIiGi iωi 2 i=1 %n & b   1 T  i T i i T i Gi i Mi Gi Gi + i Ii i q. = q˙ ˙ 2 i=1

T =

We note that

%n & b    ∂T i T i i T i Gi i = Mi Gi Gi + i Ii i q. ˙ ∂ q˙ i=1

(5.208)

(5.209)

For a system of rigid bodies, we had previously defined M (q) 

nb    Mi iTGi iGi + iTi iIiGi ii .

(5.210)

i=1

So, d ∂T = M q¨ + M˙ q˙ dt ∂ q˙ and

(5.211)

⎛

⎞ q˙ q˙T ∂M ∂q1 ∂T 1⎜ . ⎟ .. ⎟ = ⎜ ⎠. ∂q 2 ⎝ ∂M T q˙ ∂qn q˙

(5.212)

The potential energy, V , is given by V =

nb 

nb  * + Mi irGi , gieˆ03 = g Mi irGTi ieˆ03 .

i=1

(5.213)

i=1

So, b  ∂V Mi iTGi ieˆ03 . =g ∂q i=1

n

(5.214)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

135

5.3 Hamilton’s Principle of Least Action

For a system of rigid bodies, we had previously defined nb  g(q)  g Mi iTGi ieˆ03 .

(5.215)

i=1

Thus, ⎛

⎞ q˙T ∂M q˙ ∂q1 ∂T ∂V 1⎜ . ⎟ d ∂T . ⎟ + g(q). − + = M q¨ + M˙ q˙ − ⎜ τ= dt ∂ q˙ ∂q ∂q 2⎝ . ⎠ q˙ q˙T ∂M ∂qn

(5.216)

Defining b(q, q) ˙ in an alternate, but consistent, manner as previously defined: ⎛

⎞ q˙ q˙T ∂M ∂q1 1⎜ . ⎟ . ⎟: b(q, q) ˙  M˙ q˙ − ⎜ 2⎝ . ⎠ q˙ q˙T ∂M ∂qn

(5.217)

τ = M (q)q¨ + b(q, q) ˙ + g(q).

(5.218)

we have

Example: A gimballed gyroscope is depicted in Figure 5.11. The generalized coordinates of the 2-axis gimbal are q1 and q2 , while the spin angle of the gyroscope is q3 . A

Figure 5.11 A gyroscope supported by a two-axis gimballed frame. The generalized coordinates,

q1 and q2 , parameterize the gimbal frame, and q3 is the rotor angle. The rotor diameter is r.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

136

Zeroth-Order Variational Principles

zyx Euler sequence will be used to represent the orientation of the system. So, Q = Qz (q1 ),

(5.219)

Q = Qz (q1 )Qy (q2 ),

(5.220)

Q = Qz (q1 )Qy (q2 )Qx (q3 ).

(5.221)

0 1 0 2 0 3

The positions of the proximal ends and centers of mass of the links are given by r1 = 0,

0

(5.222)

rG1 = r1 − lG1 eˆ3 ,

0

0

(5.223)

r2 = r1 + 0,

0

0

(5.224)

rG2 = r2 − l Qeˆ1 ,

(5.225)

r3 = r3 + l Qeˆ1 ,

(5.226)

0

0

0

0 G2 2

0

0 32

rG3 = r3 .

0

0

(5.227)

We can compute the angular velocity in the local frame by noting that ˙ 1 = 01QT 01Q, 2 0 T0 ˙  = Q Q, 1

2

2

3

(5.228) (5.229)

2

T0

˙ 3 = 03Q 3Q.

(5.230)

Carrying this operation out, we determine ⎛

⎞ 0 1 ω1 = ⎝ 0 ⎠ q˙1 and

⎛ 2

⎜ ω2 = ⎝

− sin(q2 )q˙1 q˙2

(5.231)

⎞ ⎟ ⎠

(5.232)

cos(q2 )q˙1 and

⎛ 3

q˙3 − sin(q2 )q˙1

⎞

⎟ ⎜ ω3 = ⎝ cos(q2 ) sin(q3 )q˙1 + cos(q3 )q˙2 ⎠

(5.233)

cos(q2 ) cos(q3 )q˙1 − sin(q3 )q˙2 Alternately, we could have arrived at the same results by propagating the rotation rates forward, where 1

ω1 = q˙1 eˆ3 ,

2

ω2 =

3

ω3 =

QTy (q2 )1ω1 QTx (q3 )2ω2

(5.234) + q˙2 eˆ2 ,

(5.235)

+ q˙3 eˆ1 .

(5.236)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.3 Hamilton’s Principle of Least Action

137

The translational velocity Jacobians are given by ∂ 0rG1 = 0, ∂q ⎛ ⎞ lG2 sin(q1 ) cos(q2 ) lG2 cos(q1 ) sin(q2 ) 0 0 r ∂ G 0 2 = ⎝ −lG2 cos(q1 ) cos(q2 ) lG2 sin(q1 ) sin(q2 ) 0 ⎠ , G2 = ∂q 0 0 lG2 cos(q2 ) ⎛ ⎞ −l3 sin(q1 ) cos(q2 ) −l3 cos(q1 ) sin(q2 ) 0 0 ∂ rG3 0 = ⎝ l3 cos(q1 ) cos(q2 ) −l3 sin(q1 ) sin(q2 ) 0 ⎠ , G3 = ∂q 0 0 −l3 cos(q2 ) 0

G1 =

and the angular velocity Jacobians are given by ⎛ ⎞ 0 0 0 1 ω ∂ 1 1 = ⎝0 0 0⎠, 1 = ∂ q˙ 1 0 0 ⎛ ⎞ − sin(q2 ) 0 0 2 ω ∂ 2 2 =⎝ 2 = 0 1 0⎠, ∂ q˙ cos(q2 ) 0 0 ⎛ ⎞ − sin(q2 ) 0 1 3 ω ∂ 3 3 = ⎝ cos(q2 ) sin(q3 ) cos(q3 ) 0 ⎠ . 3 = ∂ q˙ cos(q2 ) cos(q3 ) − sin(q3 ) 0

(5.237)

(5.238)

(5.239)

(5.240)

(5.241)

(5.242)

Our dynamical terms are then M (q) =

3    Mi 0TGi 0Gi + iTi iIiG ii , i=1

⎛

⎞ q˙ q˙T ∂M ∂q1 1⎜ . ⎟ .. ⎟ b(q, q) ˙ = M˙ q˙ − ⎜ ⎠, 2 ⎝ ∂M T q˙ ∂q3 q˙ g(q) = g

3 

(5.243)

Mi 0TGi eˆ3 .

(5.244)

(5.245)

i=1

The inertia tensors will be taken as ⎛1 ⎞ M 0 0 3 1 G 1 1 I1 1 = ⎝ 0 M 0 ⎠, 3 1 1 M 0 0 2 1 ⎛ ⎞ 0.125M2 0 0 2 G2 ⎠, I2 = ⎝ 0 0 3.0625M2 0 0 3.083M2 ⎛1 ⎞ 2 M r 0 0 2 3 3 G3 1 2 ⎠. I3 = ⎝ 0 h M3 + 14 M3 r2 0 12 1 2 1 2 h M + M r 0 0 3 12 4 3

(5.246)

(5.247)

(5.248)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

138

Zeroth-Order Variational Principles

We then have ⎛

M11 0

0 M22

− 12 M3 r2 sin(q2 )

0

⎜ M (q) = ⎝

⎞ − 12 M3 r2 sin(q2 ) ⎟ − 12 M3 r2 sin(q2 ) ⎠ , 1 M r2 2 3

M11 = 0.5M1 + 1.604M2 + 0.5lG22 M2 + 0.042h2 M3 + 0.5l32 M3 + 0.375M3 r2 + [(1.479 + 0.5lG22 )M2 + M3 (0.042h2 + 0.5l32 − 0.125r2 )] cos(2q2 ), M22 = (3.0625 + lG22 )M2 + M3 (0.083h2 + l32 + 0.25r2 ), ⎛ ⎞ b1 ⎠, b(q, q) ˙ =⎝ b2 1 2 − 2 M3 r cos(q2 )q˙1 q˙2

(5.249)

b1 = −2 cos(q2 )q˙2 ([(2.958 + lG22 )M2 + M3 (0.083h2 + l32 − 0.25r2 )] sin(q2 )q˙1 + 0.25M3 r2 q˙3 ), b2 = cos(q2 )q˙1 ([(2.958 + lG22 )M2 + M3 (0.083h2 + l32 − 0.25r2 )] sin(q2 )q˙1 + 0.5M3 r2 q˙3 ), ⎛ ⎞ 0 g(q) = ⎝ g(lG M2 − l3 M3 ) cos(q2 ) ⎠ . 2

(5.250) (5.251)

0 The simulation results are displayed in Figure 5.12. The values of the constants were M1 = M2 = M3 = 1, lG1 = lG2 = 1, l3 = 3, r = 1, and h = 1. The initial conditions used were

T qo = 0 −π /6 0

T q˙o = 0 0 2π /8 .

(5.252) (5.253)

The kinetic energy of the system is % 3 &  1 T  0 T0 1 i T i Gi Mi Gi Gi + i Ii i q˙ = q˙T M (q), T = q˙ ˙ 2 2 i=1 =

1 [0.5M1 + 1.604M2 + 0.5lG22 M2 + 0.042h2 M3 + 0.5l32 M3 + 0.375M3 r2 2 + (1.479M2 + 0.5lG22 M2 + 0.042h2 M3 + 0.5l32 M3 − 0.125M3 r2 ) cos(2q2 )]q˙21 + (3.063M2 + lG22 M2 + 0.083h2 M3 + l32 M3 + 0.25M3 r2 )q˙22 − M3 r2 sin(q2 )q˙1 q˙3 + 0.5M3 r2 q˙23 .

(5.254)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.3 Hamilton’s Principle of Least Action

139

Figure 5.12 (Top) Animation frames from the simulation of the gimballed gyroscope. (Bottom) Time history of the gimbal angles, q1 and q2 . The rotor of the gyroscope is spun up to 2π /8 rad/s. Precession of the gyroscope can be observed.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

140

Zeroth-Order Variational Principles

Figure 5.13 Fluctuations of kinetic and potential energy and conservation of total energy.

The potential energy (relative to z = −2.5) of the system is V =g

nb 

Mi 0rGTi eˆ3 = g[(2.5 − lG1 )M1 + 2.5M2 + 2.5M3 + (lG2 M2 − l3 M3 ) sin(q2 )],

i=1

(5.255) and the total energy is E = T + V.

(5.256)

L = T − V.

(5.257)

The Lagrangian is

Figure 5.13 shows the fluctuations of kinetic and potential energy and the conservation of total energy for the gyroscope.

5.3.6

Constrained Least Action Least action can be applied to multibody systems with auxiliary holonomic constraint equations. We introduce a set of m holonomic (and scleronomic) constraint equations, φ(q) = 0. The zeroth-order variation of the constraint equations is δφ = δq = 0,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.3 Hamilton’s Principle of Least Action

141

where the matrix (q) = ∂φ/∂q ∈ RmC ×n is the constraint Jacobian. In this case, the Principle of Least Action can be stated as δI = 0, ∀δ|δq(to ) = δq(t f ) = 0,

and

δq = 0.

(5.258)

Thus, least action seeks the path, q(t ), in configuration space that results in a stationary value of action, I, under all path variations, δq, that vanish at the endpoints and satisfy the constraints. We recall the Euler-Lagrange equations for unconstrained systems:

t f  δI = to

 ∂L d ∂L − + τ · δq dt. ∂q dt ∂ q˙

(5.259)

This makes use of the condition that the path variations vanish at the endpoints. The condition δI = 0 ∀δq|δq = 0 applied to (5.259) implies the following orthogonality relation at any instant:   d ∂L ∂L − − τ · δq = 0 dt ∂ q˙ ∂q

(5.260)

(5.261)

∀δq ∈ ker(). Thus,



d ∂L ∂L − −τ dt ∂ q˙ ∂q



∈ ker()⊥ = im(T ).

(5.262)

This implies the familiar constrained Euler-Lagrange equations: d ∂L ∂L − = τ + T λ. dt ∂ q˙ ∂q

(5.263)

Identical equations could have been obtained by embedding the constraints directly in the Lagrangian. In this case the Lagrangian in (5.259) would be replaced by the augmented Lagrangian with the constraints adjoined, ˙ λ)  L(q, q) ˙ + λT φ(q), Laug (q, q,

(5.264)

and the stationary value of I would be sought for all variations that vanish at the endpoints. We note that δLaug = δL + λT δφ = δL + λT δq

(5.265)

∂Laug d ∂L ∂L d ∂Laug − = − − T λ = τ. dt ∂ q˙ ∂q dt ∂ q˙ ∂q

(5.266)

and

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

142

Zeroth-Order Variational Principles

In standard matrix form we have M (q)q¨ + b(q, q) ˙ + g(q) = τ + T λ,

(5.267)

subject to φ(q) = 0. For the stationary value of I to correspond to a minimum requires that the first-order variation of I be greater than or equal to zero. This condition is shown to be satisfied (Vujanovic and Atanackovic 2004) for sufficiently small time intervals, [to, t f ], if the following is satisfied:

t f δ q˙T Mδ q˙ dt ≥ 0,

(5.268)

to

which corresponds to M being positive definite over the actual path. For classical Lagrangian systems this condition is met. Since (5.267) forms a set of second-order differential equations, it is appropriate to complement it with the second derivative of the constraint equations, ˙ q˙ = 0. φ¨ = q¨ + 

5.4

Canonical Hamiltonian Formulation

5.4.1

Unconstrained Case

(5.269)

We can express a set of so-called canonical equations of motion by defining an additional set of states, the generalized momenta, p(q, q) ˙ 

∂L . ∂ q˙

(5.270)

In principle, we can invert this expression to represent the generalized velocities in terms of the generalized coordinates and the generalized momenta. With (5.270) we can express (5.169) as ∂L . ∂q

(5.271)

∂L = p˙ − τ. ∂q

(5.272)

τ = p˙ − Thus,

Given that L = L(q, q), ˙ the total differential of the Lagrangian is dL =

∂L ∂L · dq + · d q. ˙ ∂q ∂ q˙

(5.273)

Substituting (5.270) and (5.272) into (5.273), we have dL = ( p˙ − τ) · dq + p · d q. ˙

(5.274)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.4 Canonical Hamiltonian Formulation

143

Defining the Hamiltonian, H, as H  p · q˙ − L,

(5.275)

we note that the total differential of the Hamiltonian is dH = d(p · q˙ − L) = d(p · q) ˙ − dL = (τ − p) ˙ · dq + q˙ · d p.

(5.276)

Given that we can express H = H(q, p) by replacing the generalized velocities in (5.275) with expressions in terms of the generalized momenta, we have dH =

∂H ∂H · dq + · d p. ∂q ∂p

(5.277)

Comparing this with (5.276), we note that ∂H = τ − p˙ ∂q

∂H = q. ˙ ∂p

and

(5.278)

The canonical equations are thus the 2n first-order differential equations q˙ =

∂H ∂p

p˙ = τ −

(5.279) ∂H . ∂q

(5.280)

where the state vector is ( q p )T . It is noted that, in practice, computing the Hamiltonian from the Lagrangian will result in an expression for the Hamiltonian in terms of the generalized coordinates, velocities, and momenta rather than just the generalized coordinates and momenta (canonical states). The procedure for expressing the Hamiltonian exclusively in terms of the generalized coordinates and momenta as follows: 1. Express the generalized momenta, p, as a function of the generalized coordinates, q, and the generalized velocities, q, ˙ using p(q, q) ˙ =

∂L . ∂ q˙

2. Invert this expression to represent the generalized velocities as a function of the generalized coordinates and the generalized momenta. That is, determine q˙ = f (q, p). 3. Compute the Hamiltonian, H, using the Lagrangian H(q, q, ˙ p) = p · q˙ − L(q, q). ˙ 4. Express the Hamiltonian as a function of the generalized coordinates and the generalized momenta by replacing the generalized velocities with expressions in terms of the generalized momenta using q˙ = f (q, p).

Numerical Integration A first-order method for integrating Hamilton’s equations can be summarized as shown in Algorithm 5.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

144

Zeroth-Order Variational Principles

Algorithm 5 First-order method for integrating Hamilton’s equations 1:

q0 = qo {initialization}

2:

p0 = po {initialization}

3:

for i = 0 to ns − 1 do q˙i =

5:

p˙ i = − ∂H | ∂q i

6:

pi+1 = pi + p˙ i t

7:

qi+1 = qi + q˙i t

8:

5.4.2

∂H | ∂p i

4:

end for

Auxiliary Constraints As in the unconstrained case, we can express a set of canonical equations of motion. Beginning with the constrained Euler-Lagrange equations, τ=

d ∂L ∂L − − T λ, dt ∂ q˙ ∂q

(5.281)

and following the same procedure as for the unconstrained case, we have the 2n firstorder differential equations q˙ =

∂H ∂p

p˙ = τ + T λ −

(5.282) ∂H , ∂q

(5.283)

complemented by the m constraint equations q˙ = 0. This yields a set of 2n + m firstorder differential equations. In practice the integration of the forward dynamics would also require constraint stabilization to mitigate drift in the constraints. Unlike secondorder systems, where drift occurs at the position and velocity levels, we only need to be concerned with drift at the position level (Naudet et al. 2003). We can replace our original differential constraint equation with φ˙ + αφ = 0,

(5.284)

q˙ + αφ = 0.

(5.285)

or

Thus, the constraint stabilized canonical equations of motion in compact form are ⎞ ⎛ ⎞ ⎛ ∂H ⎞ ⎛ q˙ 1 0 0 ∂p ∂H ⎟ ⎝ 0 1 −T ⎠ ⎝ p˙ ⎠ = ⎜ . (5.286) τ − ⎝ ∂q ⎠  0 0 λ −αφ Dirac generalized the handling of constraints in Hamiltonian dynamics (Dirac 1958). If we consider the constraints to be a function of the generalized momenta as well as the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

145

5.5 Elimination of Multipliers

generalized coordinates, we have φ(q, p) = 0.

(5.287)

We can define the augmented Hamiltonian as Haug (q, p, λ)  H(q, p) − λT φ(q, p).

(5.288)

The constrained canonical equations are then ∂Haug ∂p ∂Haug p˙ = τ − ∂q

q˙ =

(5.289) (5.290)

or ∂H ∂φ − λ ∂p ∂p ∂H ∂φ p˙ = τ − + λ. ∂q ∂q

q˙ =

(5.291) (5.292)

The stabilized differential constraint equation is φ˙ + αφ = 0,

(5.293)

∂φ ∂φ q˙ + p˙ + αφ = 0. ∂q ∂p

(5.294)

or

The generalized constraint stabilized canonical equations of motion in compact form are ⎞ ⎛ ∂φ ⎛ ⎞ ⎛ ∂H ⎞ 1 0 ∂p ∂p ⎟ q˙ ⎜ ⎜ ∂φ ⎟ ⎝ ⎠ ∂H ⎟ ⎜ 0 1 − = . (5.295) p ˙ τ − ⎝ ∂q ⎠ ∂q ⎠ ⎝ ∂φ ∂φ λ 0 −αφ ∂q ∂p

5.5

Elimination of Multipliers The Lagrange multipliers can be eliminated from (5.86) by first expressing the zerothorder variational equation: τC · δq + (τ − M q¨ − b − g) · δq = 0.

(5.296)

By restricting the variations to constraint-consistent virtual displacements, we have τC · δq + (τ − M q¨ − b − g) · δq = 0 ∀δq ∈ ker().

(5.297)

Recalling (5.83) we note that the generalized constraint forces produce no virtual work under virtual displacements that are consistent with the constraints. Thus, the term

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

146

Zeroth-Order Variational Principles

τC · δq vanishes from (5.297), and we have the orthogonality relation (M q¨ + b + g − τ) · δq = 0 ∀δq ∈ ker().

(5.298)

We now define a matrix, W ∈ Rn×p , whose columns span the null space of . This implies that im(W ) = ker(). Thus, W = 0 and W T T = 0. In this manner, W orthogonally complements . That is, im(W ) = ker() = im(T )⊥ .

(5.299)

Geometrically, im(W ) represents the tangent space of the constrained-motion manifold, Q p (see Figure 5.5). These geometric properties are discussed in further detail in Blajer (1997) and Jungnickel (1994). While not required for the subsequent analysis, we specify that the columns of W be mutually orthogonal and thus form an orthogonal basis, C, for the null space of . The constraint-consistent virtual displacements, δq ∈ ker(), can then be expressed in terms of the virtual displacements of a minimal set of p independent coordinates, q p : δq = W δq p .

(5.300)

Using this relationship, we can express (5.298) over all possible variations of a minimal set of coordinates: (W T M q¨ + W T b + W T g − W T τ) · δq p = 0, ∀δq p ∈ R p ,

(5.301)

⇓ W T τ = W T M q¨ + W T b + W T g. ˙ q˙ p , we can express (5.301) as Noting that q˙ = W q˙ p and q¨ = W q¨ p + W τ p = M p (q)q¨ p + b p (q, q˙ p ) + g p (q),

(5.302)

where M p (q) = W T MW ,

(5.303)

˙ q˙ p , b p (q, q˙ p ) = W b + W M W T

T

(5.304)

g p (q) = W g,

(5.305)

τ p = W τ.

(5.306)

T T

The approach outlined here is consistent with the projection method of Blajer (1997). This approach was also used by Russakow et al. for application to serial-to-parallel chain manipulators (Russakow, Khatib, and Rock 1995). We note that (5.302) includes a mix of our initial set of n generalized coordinates, q, as well as the minimal set of p independent coordinates, q p . Since the constraints are holonomic, we would expect there to be a mapping, in principle, which could be derived from the constraints that would yield q = q(q p ). In this case W could be computed explicitly from the mapping rather than computing the null space of ; that is, W = ∂q/∂q p . Additionally, the terms

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.6 Exercises

147

in (5.302) could be expressed as functions of q p rather than q. Since q p are independent coordinates, the constraints would be implicitly addressed and the resulting system would be unconstrained with respect to configuration space. However, finding the mapping q = q(q p ) would be difficult in general. In such cases a null space method or a coordinate partitioning method (Wehage and Haug 1982) would need to be used to compute W . Additionally, the generalized coordinates, q p , and the generalized forces, τ p , do not necessarily have a natural and physically intuitive meaning, making it difficult to standardize their use in a numerical algorithm. This is in contrast to the coordinates, q, which are chosen specifically to describe the system in the most natural and physically intuitive manner. It is usually desirable to select q in a manner that preserves the physical meaning of the generalized forces as torques about individual joints. Often when using a minimal set of coordinates, this is not the case, since a single generalized coordinate may influence multiple joint displacements. Therefore, from an algorithmic perspective, it is often preferable to deal with a nonminimal but standardized set of generalized coordinates (like joint angles) that are amenable to numerical formulation and to compute the dynamical terms corresponding to that kinematic parametrization.

5.6

Exercises 1. Consider the serial chain robot from Section 4.2, Exercise 3 (shown in Figure 5.14). Recall that link lengths are l1 and l2 . The link radii are r1 and r2 , the link masses are M1 and M2 , and the link inertia tensors are ⎛1 ⎞ M r2 0 0 2 i i i Gi 1 2 ⎠ Ii = ⎝ 0 l Mi + 14 Mi ri2 0 12 i 1 2 1 2 l Mi + 4 Mi ri 0 0 12 i

Figure 5.14 A 2 degree-of-freedom serial chain robot parameterized by the generalized

coordinates, q1 and q2 , with generalized forces, τ1 and τ2 (Exercises 1, 5, and 7). The link lengths are l1 and l2 . The centers of mass are at the geometric centers of the links. The link radii are r1 and r2 , the link masses are M1 and M2 , and the link principal inertia components are G G i Gi Ii11 = 12 Mi ri2 and iIi22i = iIi33i = 121 li2 Mi + 14 Mi ri2 for links i = 1, 2.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

148

Zeroth-Order Variational Principles

Figure 5.15 A two-link planar slider-crank mechanism (Exercises 2, 6, and 8). The unconstrained

system is parameterized by the generalized coordinates, q1 and q2 , with generalized forces, τ1 and τ2 . The link lengths are each l and the link masses are each M. The centers of mass are at the geometric centers of the links. The link rotational inertias are taken to be zero. Link 2 is constrained from translating in the yˆ direction.

for links i = 1, 2. (a) Compute the mass matrix, M (q). (b) Compute the vector of centrifugal and Coriolis forces, b(q, q). ˙ (c) Compute the vector of gravity forces, g(q). 2. Consider the two-link planar slider-crank mechanism shown in Figure 5.15. The link lengths are each l and the link masses are each M. The link rotational inertias are taken to be zero. (a) How many degrees of freedom does the constrained system have? (b) Compute the mass matrix, M (q), for the unconstrained system. (c) Compute the vector of centrifugal and Coriolis forces, b(q, q), ˙ for the unconstrained system. (d) Compute the vector of gravity forces, g(q), for the unconstrained system. (e) Express the constraint equations, φ(q), for the loop closure. (f) Compute the constraint matrix, (q). 3. Consider the planar four-bar linkage shown in Figure 5.16. The link lengths are each l and the link masses are each M. The link rotational inertias are taken to be zero. (a) How many degrees of freedom does the constrained system have? (b) Compute the mass matrix, M (q), for the unconstrained system. (c) Compute the vector of centrifugal and Coriolis forces, b(q, q), ˙ for the unconstrained system. (d) Compute the vector of gravity forces, g(q), for the unconstrained system. (e) Express the constraint equations, φ(q) for the loop closure. (f) Compute the constraint matrix, (q). 4. Consider the three-link planar slider-crank mechanism shown in Figure 5.17. The unconstrained system is identical to the four-bar linkage of Exercise 3. (a) How many degrees of freedom does the constrained system have?

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

5.6 Exercises

149

Figure 5.16 A planar four-bar linkage (Exercise 3). The unconstrained system is parameterized by

the generalized coordinates, q1 , q2 , and q3 , with generalized forces, τ1 , τ2 , and τ3 . The link lengths are each l and the link masses are each M. The centers of mass are at the geometric centers of the links. The link rotational inertias are taken to be zero. The link 3 endpoint is constrained from translating.

(b) If not already completed, compute the mass matrix, M (q), the vector of centrifugal and Coriolis forces, b(q, q), ˙ and the gravity forces, g(q), for the unconstrained system. (c) Express the constraint equations, φ(q) for the loop closure. (d) Compute the constraint matrix, (q). 5. Consider the serial chain robot from Exercise 1. (a) Compute the kinetic energy, T , of the system. (b) Compute the potential energy, V , of the system.

Figure 5.17 A three-link planar slider-crank mechanism (Exercise 4). The unconstrained system is

parameterized by the generalized coordinates, q1 , q2 , and q3 , with generalized forces, τ1 , τ2 , and τ3 . The link lengths are each l and the link masses are each M. The centers of mass are at the geometric centers of the links. The link rotational inertias are taken to be zero. Link 3 is constrained from rotating as well as translating in the yˆ direction.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

150

Zeroth-Order Variational Principles

(c) Compute the Lagrangian, L, of the system. (d) Generate the equations of motion, directly from the Lagrangian, using the EulerLagrange equations. 6. Consider the two-link planar slider-crank mechanism from Exercise 2. (a) Compute the kinetic energy, T , of the system. (b) Compute the potential energy, V , of the system. (c) Compute the Lagrangian, L, of the system. (d) Generate the equations of motion, directly from the Lagrangian, using the EulerLagrange equations. (e) Express the constrained equations of motion. 7. Consider the serial chain robot from Exercise 1. (a) Express the generalized momenta, p, as a function of the generalized coordinates, q, and the generalized velocities, q. ˙ Invert this expression to represent the generalized velocities as a function of the generalized coordinates and the generalized momenta. (b) Compute the Hamiltonian, H, using the Lagrangian. Express the Hamiltonian as a function of the generalized coordinates and the generalized momenta by replacing the generalized velocities with expressions in terms of the generalized momenta. (c) Generate the equations of motion, directly from the Hamiltonian, using Hamilton’s canonical equations. 8. Consider the two-link planar slider-crank mechanism from Exercise 2. (a) Express the generalized momenta, p, as a function of the generalized coordinates, q, and the generalized velocities, q. ˙ Invert this expression to represent the generalized velocities as a function of the generalized coordinates and the generalized momenta. (b) Compute the Hamiltonian, H, using the Lagrangian. Express the Hamiltonian as a function of the generalized coordinates and the generalized momenta by replacing the generalized velocities with expressions in terms of the generalized momenta. (c) Generate the equations of motion, directly from the Hamiltonian, using Hamilton’s canonical equations. (d) Express the constrained equations of motion.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:24:39, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.007

6

First-Order Variational Principles

In the previous chapter we addressed zeroth-order variational principles rooted in the fundamental principle of d’Alembert. In this chapter we will focus on the first-order variation of displacement. We will begin with Jourdain’s Principle of Virtual Power (Jourdain 1909). The principle is based on the notion of virtual velocity. The derivation of the equations of motion for particles and rigid bodies using Jourdain’s Principle closely parallels the derivation of the equations of motion using d’Alembert’s Principle in the previous chapter. While Jourdain’s Principle is analogous to d’Alembert’s Principle, it can be seen as an independent principle of analytical dynamics. Therefore, the material in the previous chapter is not a prerequisite to this chapter, and this chapter will be treated in a stand-alone manner.

6.1

Virtual Velocities Virtual velocities refer to all velocities of a system that satisfy the scleronomic constraints of the system. In the case of virtual velocities, time and position are frozen or stationary.

6.2

Jourdain’s Principle of Virtual Power p r i n c i p l e 6.1 The virtual power of a system is stationary. That is, δP = 0.

(6.1)

Additionally, the constraints of the system generate no virtual power, δPc = 0.

(6.2)

This is known as Jourdain’s Principle. As with d’Alembert’s Principle, it is noted that while Jourdain’s Principle can be seen as providing an alternate statement of Newton’s second law, for interacting bodies, a law of action and reaction (Newton’s third law) is still needed. Therefore, when we use Jourdain’s Principle to derive the equations of motion for systems of particles/bodies, we will invoke the law of action and reaction.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

152

First-Order Variational Principles

6.2.1

A Single Particle Jourdain’s Principle for a single point mass with a discrete set of n f external forces, { f1 , . . . , fn f }, acting on it is expressed as δP =

nf 

fi · δv − Mgeˆ3 · δv − Ma · δv = 0

i=1

(6.3)

∀δv ∈ R , 3

where δv represents the velocity variations. During these variations, time and position are stationary. That is, δt = 0 and δr = 0. More concisely, we can express  nf   δP = fi − Mgeˆ3 − Ma · δv = 0 (6.4) i=1 ∀δv ∈ R3 , which implies nf 

fi − Mgeˆ3 − Ma = 0.

(6.5)

i=1

6.2.2

A Single Rigid Body Jourdain’s Principle for a single rigid body with a discrete set of external forces, fi , and moments, ϕ j , acting on it is expressed as δP =

nf 

fi · δv Pi +

i=1

nϕ 

ϕ j · δω − Mgeˆ3 · δv G − MaG · δv G − (I G α + ω × I G ω) · δω = 0,

j=1

∀δv G ∈ R3 ,

and

∀δω ∈ R3 ,

(6.6) where the δv and δω terms represent all velocity variations consistent with the rigidbody constraint. This is depicted in Figure 6.1. During these variations, time, position, and orientation are stationary. That is, δt = 0, δr = 0, and δθ = 0. Jourdain’s Principle states that the virtual power associated with all internal forces and moments consistent with the rigid-body constraint is zero (Pc = 0). We further note that −→ − v − ω × d−→ = δv + δω × d−→ , δv Pi = (v G + δv G ) + (ω + δω) × dGP G G GPi GPi i

(6.7)

Therefore, nf  i=1

fi · δv Pi =

nf  i=1

 nf  nf   − → fi · (δv G + δω × dGPi ) = fi · δv G + fi · (δω × d−GP→i ). i=1

i=1

(6.8)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

153

Figure 6.1 A virtual velocity of a rigid body consisting of a translational, δv G , and rotational, δω, velocity variation. We are concerned with all such velocity variations consistent with the rigid-body constraint. During these variations, time, position, and orientation are stationary.

Since a · (b × c) = (c × a) · b, nf 

fi · (δω × d−GP→i ) =

i=1

nf 

(d−GP→i × fi ) · δω =

 nf 

i=1

 d−GP→i × fi · δω.

(6.9)

i=1

So, nf 

 nf   nf    fi · δv Pi = fi · δv G + d−GP→i × fi · δω.

i=1

i=1

(6.10)

i=1

Substituting (6.10) into (6.6), we get  nf   δP = fi − Mgeˆ3 − MaG · δv G i=1

⎛ ⎞ nf nϕ   +⎝ d−GP→i × fi + ϕ j − I G α − ω × I G ω⎠ · δω = 0, i=1

(6.11)

j=1

or ⎛ ⎜ ⎜ nf ⎝ i=1

nf

⎞

fi − Mgeˆ3 − MaG

i=1

d−GP→i × fi +

nϕ

ϕ j − I Gα − ω × I Gω

⎟ ⎟· ⎠



δv G δω

 = 0,

(6.12)

j=1

∀δv G ∈ R3 ,

and

∀δω ∈ R3 ,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

154

First-Order Variational Principles

which implies nf 

fi − Mgeˆ3 − MaG = 0

(6.13)

i=1 nf 

d

−→ GPi

× fi +

i=1

6.2.3

nϕ 

ϕ j − I G α − ω × I G ω = 0.

(6.14)

j=1

A System of Particles We now apply Jourdain’s Principle to a system of n p particles with a discrete set of forces acting on them. The virtual power associated with a given particle i is given by ⎛ ⎞ np  δPi = ⎝ fi j ⎠ · δv i − Mi (ai + geˆ3 ) · δv i = 0 (6.15) j=0 ∀δv i ∈ R3 for i = 1, . . . , n p . The term fi j is the force that particle j exerts on particle i, where fi0 is the force exerted by the ground (inertial reference frame) on the ith particle. Equation (6.15) is true for any particle i in the system, under all independent variations, δv i . If we sum (6.15) over all particles, we obtain ⎛ ⎞ np np np np     j⎠ ⎝ δPi = fi · δv i − Mi (ai + geˆ3 ) · δv i = 0. (6.16) δP = i=1

i=1

j=0

i=1

The first summation is associated with the virtual power performed by the interparticle reaction forces. It will be useful to rearrange the summation so as to pair up the equal and opposite reaction forces, fi j and f ji . Doing this, we have ⎛ ⎞ np np n p −1 n p np np       ⎝ fi j ⎠ · δv i = ( fi j · δv i + f ji · δv j ) + fii · δv i − f0j · δv 0 . i=1

j=0

i=0 j=i+1

i=1

j=1

(6.17) Since fi = 0 and δv 0 = 0, we have ⎛ ⎞ np np n p −1 n p     ⎝ fi j ⎠ · δv i = ( fi j · δv i + f ji · δv j ). i

i=1

j=0

We note that f ji = − fi j . Thus, ⎛ ⎞ np np n p −1 n p     j⎠ ⎝ fi · δv i = fi j · (δv i − δv j ). i=1

j=0

(6.18)

i=0 j=i+1

(6.19)

i=0 j=i+1

This reflects the virtual power done by interparticle reaction forces, summed over all particles.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

155

If we consider that the particles are subject to a set of holonomic constraints the positions, ri , and in turn velocities, v i , are not independent. In this case we will assume that the velocities can be expressed in terms of a set of nq independent generalized velocities, q. ˙ If we now consider only the variations, δv i , that are consistent with the kinematic constraints, then (6.19) reflects a projection of the interparticle forces on the direction of the interparticle motion. Jourdain’s Principle states that the virtual power associated with all forces orthogonal to the interparticle motion (reaction forces) is zero (Pc = 0), and only the generalized force acting in the direction of interparticle motion produces virtual power. Thus, n p −1

np  

fi j · (δv i − δv j ) =

nq 

τi · δ q˙i ,

(6.20)

Mi (ai + geˆ3 ) · δv i = 0

(6.21)

i=0 j=i+1

i=1

and (6.16) can be expressed as nq 

τi · δ q˙i −

np 

i=1

i=0

for all variations, δv i , that are consistent with the kinematic constraints. Expressing the variations in terms of variations in the generalized velocities (with stationary displacement and time), we have ˙ δv i = i δ q.

(6.22)

So, (6.21) can be expressed as τ · δ q˙ −

np 

Mi (ai + geˆ3 ) · (i δ q) ˙ =0

i=1

(6.23)

∀δ q˙ ∈ Rn , or

% τ−

np 

& i (Mi ai + Mi geˆ3 ) · δ q˙ = 0 T

i=1

(6.24)

∀δ q˙ ∈ Rn , which implies τ=

np 

Ti (Mi ai + Mi geˆ3 ).

(6.25)

¨ ai = i q,

(6.26)

i=1

Noting that

we have τ=

 np  i=1



nq  Mi i i q¨ + g Mi Ti eˆ3 . T

(6.27)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

156

First-Order Variational Principles

For a system of particles, we had previously defined M (q) 

np 

Mi Ti i

(6.28)

i=1 np

 g(q)  g Mi Ti eˆ3 .

(6.29)

i=1

So, τ = M (q)q¨ + g(q).

6.2.4

(6.30)

A System of Rigid Bodies We now apply Jourdain’s Principle to a system of nb rigid bodies forming a serial chain, with a discrete set of forces acting on them. The virtual power associated with a given body i is given by δPi = fii−1 · δv ii−1 + fii+1 · δv ii+1 − Mi (aGi + geˆ03 ) · δv Gi + ϕii−1 · δωi + ϕii+1 · δωi − (IiGi αi + ωi × IiGi ωi ) · δωi = 0, ∀δv Gi ∈ R , 3

and

(6.31)

∀δωi ∈ R , 3

for i = 1, . . . , nb . The term fii−1 is the force that body i − 1 exerts on body i. Likewise, ϕii−1 is the moment that body i − 1 exerts on body i. The term v ii−1 is the velocity of the point on body i to which body i − 1 attaches. Equation (6.31) is true for any rigid body i in the system, under all independent variations, δv Gi and δωi (we note that δv ii−1 and δv ii+1 are functions of δv Gi and δωi due to the rigid-body constraint). If we sum (6.31) over all rigid bodies, we obtain δP =

nb  i=1

δPi =

nb 

( fii−1 · δv ii−1 + fii+1 · δv ii+1 )

i=1

−

nb 

Mi (aGi + geˆ03 ) · δv Gi +

i=1

−

nb 

nb 

(ϕii−1 · δωi + ϕii+1 · δωi )

i=1

(IiGi αi + ωi × IiGi ωi ) · δωi = 0.

(6.32)

i=1

The first summation is associated with the virtual power performed by the interlink reaction forces. It will be useful to rearrange the summation so as to pair up the equal i , acting through each joint i. Doing this, we and opposite reaction forces, fii−1 and fi−1 have nb  i=1

( fii−1 · δv ii−1 + fii+1 · δv ii+1 ) =

nb 

i ( fii−1 · δv ii−1 + fi−1 · δv ii−1 ) + fnnbb +1 · δv nnbb +1 − f01 · δv 10 .

i=1

(6.33)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

157

Since fnnbb +1 = 0 and δv 10 = 0, we have nb 

( fii−1 · δv ii−1 + fii+1 · δv ii+1 ) =

i=1

nb 

i ( fii−1 · δv ii−1 + fi−1 · δv ii−1 ).

(6.34)

i=1

i = − fii−1 . Thus, We note that fi−1

nb 

nb 

( fii−1 · δv ii−1 + fii+1 · δv ii+1 ) =

i=1

fii−1 · (δv ii−1 − δv ii−1 ).

(6.35)

i=1

This reflects the virtual power done by reaction forces at each joint, summed over all joints. As with the reaction forces, it will be useful to rearrange the summation associated with the virtual power performed by the interlink reaction moments so as to pair up the i , acting about each joint. Using the equal and opposite reaction moments, ϕii−1 and ϕi−1 same procedure as with the reaction forces, we have nb 

(ϕii−1 · δωi + ϕii+1 · δωi ) =

i=1

nb 

i (ϕii−1 · δωi + ϕi−1 · δωi−1 )

i=1

=

nb 

ϕii−1 · (δωi − δωi−1 ).

(6.36)

i=1

This reflects the virtual power done by reaction moments at each joint, summed over all joints. So, the total virtual power performed by the interlink reaction forces and moments can be expressed compactly as nb 

( fi

i−1

· δv

i=1

=

nb  i=1



i−1 i

fii−1 ϕii−1

+ fi

i+1

  ·

· δv ) + i+1 i

nb 

(ϕii−1 · δωi + ϕii+1 · δωi )

i=1

δv ii−1 − δv ii−1 δωi − δωi−1

 .

(6.37)

If we now consider only the variations δv ii−1 , δv ii−1 , δωi , and δωi−1 that are consistent with the kinematic constraints, then (6.37) reflects a projection of the interlink forces and moments on the direction of the joint motion. Jourdain’s Principle states that the virtual power associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Pc = 0) and only the generalized force acting in the direction of joint motion produces virtual power. Thus,     nq nb   fii−1 δv ii−1 − δv ii−1 τi · δ q˙i , (6.38) · = ϕii−1 δωi − δωi−1 i=1 i=1 and (6.32) can be expressed as nq  i=1

τi · δ q˙i −

nb  i=1

Mi (aGi + geˆ03 ) · δv Gi −

nb 

(IiGi αi + ωi × IiGi ωi ) · δωi = 0

(6.39)

i=1

for all variations, δv Gi and δωi , that are consistent with the kinematic constraints.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

158

First-Order Variational Principles

Using Jourdain’s Principle, we can address a system of nb rigid bodies forming a branching chain in a similar manner. With a serial chain the parent/child structure is implicit to the numbering scheme. Every link i has a single parent link, i − 1, at its proximal end and a single child link, i + 1, at its distal end (except for the nth link). The ith joint is at the proximal end of the ith link. With a branching chain the numbering of links is more arbitrary, without a parent/child structure implicit to the numbering scheme. We can explicitly capture the parent/child structure, however, by defining three additional parameters, λi , ci , μi j . The term λi is the parent link number of the ith link, ci is the number of child links for the ith link, and μi1 · · · μici are the child link numbers of the ith link. Given these parameters, the virtual power associated with a given body i is given by δPi = fiλi · δv λi i +

ci 

fi

μi j

μ

· δv i i j − Mi (aGi + geˆ03 ) · δv Gi + ϕiλi · δωi

j=1

+

ci 

(6.40)

μ

ϕi i j · δωi − (IiGi αi + ωi × IiGi ωi ) · δωi = 0,

j=1

∀δv Gi ∈ R3 ,

∀δωi ∈ R3 ,

and

λ

for i = 1, . . . , nb . The term v i i is the velocity of the point on body i to which body (parμic μ ent) λi attaches, and likewise v i i1 . . . v i i are the velocities of points on body i to which μic μ λ bodies (children) μi1 . . . μici attach. We note that δv i i and δv i i1 . . . δv i i are functions of δv Gi and δωi due to the rigid-body constraint. If we sum (6.40) over all rigid bodies, we obtain ⎛ ⎞ nb nb ci    μ μ ⎝ fiλi · δv λi i + δP = δPi = fi i j · δv i i j ⎠ i=1

−

nb 

i=1

Mi (aGi + geˆ03 ) · δv Gi +

i=1

−

nb 

j=1 nb 

⎛ ⎝ϕiλi · δωi +

i=1

ci 

⎞ ϕi · δωi ⎠ μi j

j=1

(IiGi αi + ωi × IiGi ωi ) · δωi = 0.

(6.41)

i=1

The term associated with the virtual power performed by the interlink reaction forces is ⎛ ⎞ nb ci   μ μ ⎝ fiλi · δv λi i + fi i j · δv i i j ⎠. (6.42) i=1

j=1

It will be useful to rearrange the summation so as to pair up the equal and opposite λ reaction forces, fi i and fλii , acting through each joint i. We can rewrite the summation of (6.42) based on considering the virtual power at each joint. The sum over all joints then gives us nb  i=1

( fiλi · δv λi i + fλii · δv iλi ) =

nb 

fiλi · (δv λi i − δv iλi ).

(6.43)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

159

6.2 Jourdain’s Principle of Virtual Power

The term associated with the virtual power performed by the interlink reaction moments is nb 

⎛

ci 

⎝ϕiλi · δωi +

i=1

⎞ ϕi i j · δωi ⎠. μ

(6.44)

j=1

In a similar manner as before, we can rewrite this summation based on considering the virtual power at each joint. The sum over all joints then gives us nb 

(ϕ · δωi + ϕ · δωλi ) = λi i

i λi

nb 

i=1

ϕiλi · (δωi − δωλi ).

(6.45)

i=1

So, the total virtual power performed by the interlink reaction forces and moments can be expressed compactly as nb 

 fiλi · δv λi i +

i=1

=

nb 



i=1

ci  j=1

λ

fi i λ ϕi i

  ·

 fi

μi j

μi j

· δv i

,+

nb 

 ϕiλi · δωi +

i=1

 λ δv i i − δv iλi . δωi − δωλi

ci 

 μi j

ϕi · δωi

j=1

(6.46)

λ

If we now consider only the variations δv i i , δv iλi , δωi , and δωλi that are consistent with the kinematic constraints, then (6.46) reflects a projection of the interlink forces and moments on the direction of the joint motion. Jourdain’s Principle states that the virtual power associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Pc = 0) and only the generalized force acting in the direction of joint motion produces virtual power. Thus, nb  i=1



λ

fi i λ ϕi i

  ·

λ

δv i i − δv iλi δωi − δωλi

 =

nq 

τi · δ q˙i ,

(6.47)

i=1

and (6.41) can be expressed as nq  i=1

τi · δ q˙i −

nb 

Mi (aGi + geˆ03 ) · δv Gi −

i=1

nb 

(IiGi αi + ωi × IiGi ωi ) · δωi = 0

(6.48)

i=1

for all variations, δv Gi and δωi , that are consistent with the kinematic constraints. Expressing the velocity variations in terms of variations in the generalized velocities (with stationary displacement and time), we have i

δv Gi = iGi δ q˙

and

i

δωi = ii δ q, ˙

(6.49)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

160

First-Order Variational Principles

where the terms are expressed in the local link frame i for convenience. So, (6.48) can be expressed as τ · δ q˙ −

nb 

Mi (iaGi + gieˆ03 ) · (iGi δ q) ˙ −

nb 

i=1

(iIiGi iαi + iωi × iIiGi iωi ) · (ii δ q) ˙ =0

i=1 n

∀δ q˙ ∈ R ,

(6.50)

or τ · δ q˙ −

nb ' (  Mi iTGi iaGi + Mi giTGi ieˆ03 + iTi (iIiGi iαi + iωi × iIiGi iωi ) · δ q˙ = 0 i=1

(6.51)

∀δ q˙ ∈ Rn . In matrix form we have % nb   i T Gi τ−

i

Ti





&

Mi iaGi + Mi gieˆ03 I αi + iωi × iIi i iωi

i Gi i i

i=1

G

· δ q˙ = 0

(6.52)

∀δ q˙ ∈ R , n

which implies τ=

nb  

i

TGi

i

Ti





i=1

Mi iaGi + Mi gieˆ03 I αi + ii iIi i iωi

i Gi i i

G

 .

(6.53)

Since this is identical to (5.53), we have τ = M (q)q¨ + b(q, q) ˙ + g(q).

6.2.5

(6.54)

Auxiliary Constraints First-Order Nonholonomic Constraints Nonholonomic constraints are constraints that are nonintegrable functions of the generalized coordinates and generalized velocities (and possibly the generalized accelerations). The condition of nonintegrability means that not all constraints that are functions of the generalized velocities are in fact nonholonomic. For example, all smooth holonomic constraints can be differentiated, yielding constraints that are functions of the generalized velocities. However, these differentiated constraints are clearly integrable back to the original holonomic constraints. First-order nonholonomic constraints are nonintegrable functions exclusively of the generalized coordinates and generalized velocities. We will address these constraints here. We previously considered the number of degrees of freedom for holonomically constrained systems. In systems with purely nonholomic constraints, all of the ndimensional configuration space is accessible; however, the system is restricted in the manner in which points in configuration space can be reached. That is, the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

161

6.2 Jourdain’s Principle of Virtual Power

nonholmomic constraints do not place a restriction on points in configuration space that are allowable, but they do place a restriction on the differential motion allowed at a point in configuration space. The number of degrees of freedom of a nonholonomically constrained system is computed in the same way as that of a holonomically constrained system. That is, p = n − m,

(6.55)

where n is the number of generalized coordinates and m is the number of independent nonholonomic constraint equations. The number of degrees of freedom refers to the dimensionality of the allowable differential motion of the system (not the dimensionality of the accessible configuration space). We now consider the general case of auxiliary first-order nonholonomic constraints. Given the multibody system τ = M (q)q¨ + b(q, q) ˙ + g(q),

(6.56)

subject to the first-order nonholonomic constraints ψ(q, q) ˙ = 0,

(6.57)

we begin by first expressing the first-order variational equation associated with Jourdain’s Principle, τC · δ q˙ + (τ − M q˙ − b − g) · δ q˙ = 0.

(6.58)

The virtual velocities, δ q, ˙ refer to all velocity variations that satisfy the constraints, while time and displacement are fixed. With δt = 0 and δq = 0, the variation of the constraint equation yields δψ =

∂ψ δ q˙ = δ q˙ = 0, ∂ q˙

(6.59)

which implies that δ q˙ ∈ ker(). Under this condition, (6.58) can be restricted to constraint-consistent virtual displacements: τC · δ q˙ + (τ − Mq − b − g) · δ q˙ = 0 ∀δ q˙ ∈ ker().

(6.60)

We have τC ⊥δ q˙ ∀δ q˙ ∈ ker().

(6.61)

Thus, the term τC · δ q˙ vanishes from (6.60), and we have the orthogonality relation (Mq + b + g − τ) · δ q˙ = 0 ∀δ q˙ ∈ ker().

(6.62)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

162

First-Order Variational Principles

The constrained multibody equations of motion, expressed in the familiar multiplier form, are thus τ = M q¨ + b + g − T λ,

(6.63)

subject to ψ(q, q) ˙ =0

⇒

ψ˙ = 0.

(6.64)

In the special case of nonholonomic constraints that are linear in q, ˙ we have ψ(q, q) ˙ = C(q)q˙ + d(q) = 0,

(6.65)

and the first-order variation is given by δψ = Cδ q˙ = 0.

(6.66)

The constrained multibody equations of motion express as τ = M q¨ + b + g − C T λ,

(6.67)

subject to C q˙ + d = 0

⇒

C˙ q˙ + C q¨ + d˙ = 0.

(6.68)

In the special case of holonomic constraints, φ(q) = 0,

(6.69)

we can differentiate once with respect to time to yield ∂φ ψ(q, q) ˙ = φ˙ = q˙ = q˙ = 0. ∂q

(6.70)

δψ = δ q˙ = 0,

(6.71)

The variation is given by

and the constrained multibody equations of motion express identically to the zerothorder variational case, τ = M q¨ + b + g − T λ,

(6.72)

subject to φ(q) = 0

⇒

φ˙ = 0, φ¨ = 0.

(6.73)

Numerical Integration Given the nonholonomic system τ = M q¨ + b + g − C T λ,

(6.74)

subject to C q˙ = 0

⇒

C q¨ + C˙ q˙ = 0,

(6.75)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

163

Algorithm 6 Second-order method for integrating the first-order nonholonomically constrained equations of motion 1: q0 = qo {initialization} 2:

q˙0 = q˙o {initialization}

for i = 0 to ns − 1 do 4: q¨i = −C¯ iC˙ i q˙i + i Mi−1 (τi − bi − gi ) − βC¯ iCi q˙i 3:

5:

T λi = −C¯ i (τi − bi − gi ) − HiC˙ i q˙i − βHiCi q˙i

6:

q˙i+1 = q˙i + q¨i t

7:

qi+1 = qi + q˙i t + 12 q¨i t 2

8:

end for

we wish to stabilize the constraints by replacing our original acceleration constraint equation with a linear combination of acceleration and velocity constraint terms, C q¨ + C˙ q˙ + βC q˙ = 0. Thus, the constraint stabilized equations of motion are      q¨ τ −b−g M − CT = , −C 0 λ C˙ q˙ + βC q˙

(6.76)

(6.77)

and the solution of this system is ¯ q˙ q¨ = −C¯C˙ q˙ + M −1 (τ − b − g) − βCC T

λ = −C¯ (τ − b − g) − HC˙ q˙ − βHC q, ˙

(6.78) (6.79)

¯ and H = (CM −1C T )−1 . The term −βHC q˙ in the expression for λ where = 1 − CC can be physically interpreted as a corrective constraint force term used to compensate for any drift in the constraints. A second-order method for integrating this system can be summarized as shown in Algorithm 6.

Example: We consider a rolling disk which illustrates nonholonomic constraints. We parameterize the system using six generalized coordinates as shown in Figure 6.2. The order of rotations will be specified as a zyx Euler sequence: Q = Qz (q4 )Qy (q5 )Qx (q6 ) = Qzyx (α, β, γ ).

O D

(6.80)

We can compute the angular velocity in the local frame by noting that D

˙  = ODQT ODQ.

(6.81)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

164

First-Order Variational Principles

Figure 6.2 A rolling disk with nonholonomic constraints. Six generalized coordinates, q1 , . . . , q6 , parameterize the position of the center of the disk and the disk orientation. Nonholonomic constraints arise out of the zero velocity (no-slip) condition imposed on the instantaneous contact point of the disk with the ground.

Carrying this operation out, we determine ⎛ ⎛ ⎞ ⎞⎛ ⎞ 0 1 − sin(q5 ) q˙4 q˙4 ⎜ ⎜ ⎟ ⎟⎜ ⎟ D ω = ⎝ cos(q5 ) sin(q6 ) cos(q6 ) 0 ⎠ ⎝ q˙5 ⎠ = E(q4 , q5 , q6 ) ⎝ q˙5 ⎠ . cos(q5 ) cos(q6 )

− sin(q6 ) 0

q˙6

(6.82)

q˙6

Alternately, we could have arrived at the same results by propagating the rotation rates forward, where ⎛ ⎞ 0 4 ⎝ ω4 = q˙4 eˆ3 = 0 ⎠ (6.83) q˙4 and

⎛ 5

⎜ ω5 = QTy (q5 )4ω4 + q˙5 eˆ2 = ⎝

− sin(q5 )q˙4 q˙5

⎞ ⎟ ⎠.

(6.84)

cos(q5 )q˙4 Finally, ⎛ D

− sin(q5 )q˙4 + q˙6

⎞

⎟ ⎜ ω = 6ω6 = QTx (q6 )5ω5 + q˙6 eˆ1 = ⎝ cos(q5 ) sin(q6 )q˙4 + cos(q6 )q˙5 ⎠ .

(6.85)

cos(q5 ) cos(q6 )q˙4 − sin(q6 )q˙5 We now express the displacement vector from the center of the disk to the contact point of the disk with the plane. As the disk rotates about the x-axis, the displacement vector keeps the same orientation in the base frame. In the disk frame, the displacement vector rotates about the x-axis by −q6 . This can be expressed as D

→ = Q (−q )[−re d−GC ˆ3 ], x 6

(6.86)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

165

6.2 Jourdain’s Principle of Virtual Power

or O

O → = QQ (−q )[−re d−GC ˆ3 ] = Qz (q4 )Qy (q5 )[−reˆ3 ]. x 6 D

(6.87)

The instantaneous velocity of the material point of the disk in contact with the plane is given by ⎛ ⎞ q˙1 ⎜ ⎟ O D O → ). vC = ⎝ q˙2 ⎠ + DQ( ω × Dd−GC (6.88) q˙3 Under the condition that the disk does not slip relative to the plane, we have O

vC = 0.

(6.89)

Evaluating this expression, we have ⎞ ⎛ q˙1 + r[sin(q4 ) sin(q5 )q˙4 − cos(q4 ) cos(q5 )q˙5 − sin(q4 )q˙6 ] ⎟ ⎜ O vC = ⎝ q˙2 − r[cos(q4 ) sin(q5 )q˙4 + sin(q4 ) cos(q5 )q˙5 − cos(q4 )q˙6 ] ⎠ = 0

(6.90)

q˙3 + r sin(q5 )q˙5 or ⎛

1

⎜ ⎝0 

0

−r cos(q4 ) cos(q5 )

r sin(q4 ) sin(q5 )

0

0

1

0 −r cos(q4 ) sin(q5 )

0

1

0

−r sin(q4 ) cos(q5 )

−r sin(q4 )

⎟ r cos(q4 ) ⎠ q˙ = C q˙ = 0,

r sin(q5 )



⎞

0



C

(6.91) which are our nonholonomic constraint equations. We now note that O

v G = OG q˙

(6.92)

ω = D q. ˙

(6.93)

and D

Evaluating the Jacobians, we have ⎛ O

1 0

⎜ G = ⎝ 0 1 0 0

0

⎞

0

0

0

0

0

0

⎟ 0⎠

1

0

0

0

(6.94)

and ⎛ D





0

0

⎜  = 0 E(q4 , q5 , q6 ) = ⎝ 0 0

0

− sin(q5 )

0

0

cos(q5 ) sin(q6 )

cos(q6 )

0 0 0 cos(q5 ) cos(q6 )

− sin(q6 )

1

⎞

⎟ 0 ⎠ , (6.95) 0

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

166

First-Order Variational Principles

where M = M OTG OG + DT DI G D, ˙ + D DI G D)q, b = DT (DI G D ˙

(6.97)

g = gM OTG eˆ3 .

(6.98)

We will take the inertia tensor to be ⎛1 ⎜ I =⎝

D G

2

Mr2 0

0

1 Mr2 4

0

0

1 Mr2 4

Evaluating the dynamical terms, we have ⎛ M 0 0 0 ⎜ 0 ⎜0 M 0 ⎜ ⎜0 0 M 0 ⎜ M =⎜ 1 2 ⎜0 0 0 Mr [3 − cos(2q5 )] 8 ⎜ ⎜ 0 ⎝0 0 0

and

0

⎛

⎟ ⎠.

0

(6.99)

0

⎞

⎟ ⎟ ⎟ ⎟ 0 ⎟ ⎟ − 12 Mr2 sin(q5 ) ⎟ ⎟ ⎟ 0 ⎠

0

0

0 0 1 Mr2 4

− 12 Mr2 sin(q5 )

0

⎞

0

0

0

(6.96)

(6.100)

1 Mr2 2

0

⎞

0

⎜ ⎟ 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ 0 ⎜ ⎟ b=⎜ 1 2 ⎟ ⎜ Mr cos(q5 )q˙5 [sin(q5 )q˙4 − q˙6 ] ⎟ ⎜ 2 ⎟ ⎜ 1 2 ⎟ ⎝ − 4 Mr cos(q5 )q˙4 [sin(q5 )q˙4 − 2q˙6 ] ⎠

(6.101)

− 12 Mr2 cos(q5 )q˙4 q˙5 and

g= 0

0 Mg 0

0

0

T

.

(6.102)

The nonholonomically constrained equations of motion express as τ = M q¨ + b + g − C T λ,

(6.103)

subject to C q˙ = 0

⇒

C˙ q˙ + C q¨ = 0.

(6.104)

We note that n = 6 and m = 3. The constrained system has p = n − m = 3 degrees of freedom. The simulation results are displayed in Figures 6.3 and 6.4. All geometric and inertial constants were chosen to be 1. That is, r = 1 and M = 1. No generalized forces were

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

167

Figure 6.3 (Top) Animation frames from the simulation of a nonholonomic rolling disk. (Bottom)

Time history of the position of the center of mass of the disk (x, y, z).

applied (τ = 0). The initial conditions used were

qo = −2.125

0

q˙o = 0

0

2.5

π cos( 12 )

0

0

0 −2.5

π 12

T

.

0

T

(6.105) (6.106)

It can easily be confirmed that these initial conditions satisfy the nonholonomic constraints, C q˙ = 0.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

168

First-Order Variational Principles

Figure 6.4 Time history of the constraint forces exerted on the disk at the contact point

(λ1 , λ2 , λ3 ).

Example: A rolling ball can be modeled in a similar way as the rolling disk (see Figure 6.5). The order of rotations will be specified as a zyx Euler sequence, as with the disk, Q = O6 Q = Qz (q4 )Qy (q5 )Qx (q6 ) = Qzyx (α, β, γ ).

O B

(6.107)

Figure 6.5 A rolling ball with nonholonomic constraints. Six generalized coordinates, q1 , . . . , q6 , parameterize the position of the center of the ball and the ball orientation. Another two generalized coordinates, q7 and q8 , parameterize the rotation of the two internal moment wheels. Nonholonomic constraints arise out of the zero velocity (no-slip) condition imposed on the instantaneous contact point of the ball with the ground.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

169

We can compute the angular velocity of the ball in the local frame in the same manner as the disk: ⎞ ⎛ − sin(q5 )q˙4 + q˙6 ⎟ ⎜ B (6.108) ωB = 6ω6 = ⎝ cos(q5 ) sin(q6 )q˙4 + cos(q6 )q˙5 ⎠ . cos(q5 ) cos(q6 )q˙4 − sin(q6 )q˙5 The displacement vector from the center of the ball to the contact point of the ball with the plane is different than for the disk. The displacement vector remains fixed in the base frame. Thus, O

→ = −re d−GC ˆ3 .

(6.109)

The instantaneous velocity of the material point of the disk in contact with the plane is given by ⎛ ⎞ q˙1 

O →. vC = ⎝ q˙2 ⎠ + OBQBωB × Od−GC (6.110) q˙3 Evaluating the no-slip condition yields ⎛ 1 0 0 0 −r cos(q4 ) ⎜ O vC = ⎝ 0 1 0 0 −r sin(q4 )

−r sin(q4 ) cos(q5 )

0 0 1 0

0

⎞⎛

⎞ q1 ⎟⎜ . ⎟ r cos(q4 ) cos(q5 ) ⎠ ⎝ .. ⎠ = 0.

0

(6.111)

q6

We will consider that the ball has two internal actuators that generate torque at joints 7 and 8. These actuators behave as moment wheels. We will assume all masses to be the same (unity) and take the inertia tensors of the bodies associated with joints 6, 7, and 8 to be ⎛ ⎞ 0.5 0 0 ⎜ ⎟ 6 G I6 = ⎝ 0 0.5 0 ⎠ , (6.112) 0 ⎛

0.25

⎜ I =⎝ 0

7 G 7

⎛

0

0.5 0

0.25

0

⎟ 0 ⎠,

0

0

0.5

0.25

0

0

⎜ I =⎝ 0

8 G 8

0

0.5 0

⎞ (6.113)

⎞

⎟ 0 ⎠.

(6.114)

0.25

The kinematics are given by Q = O6 QQz (q7 )

(6.115)

Q = QQz (q7 )Qy (q8 ).

(6.116)

O 7 O 8

O 6

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

170

First-Order Variational Principles

Figure 6.6 (Top) Animation frame from the simulation of the nonholonomic rolling ball. (Bottom) Time history of the position of the center of mass of the ball (x, y, z).

The angular velocities of the moment wheels (frames 7 and 8) can be computed from 7 = O7 QT O7 Q˙

(6.117)

˙ 8 = O8 QT O8 Q.

(6.118)

7

8

The angular velocity Jacobians, 66 , 77 , and 88 , follow from the angular velocities. For the translational velocity Jacobians, noting that all the mass centers are concentric,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.2 Jourdain’s Principle of Virtual Power

171

we have ⎛ O

1 0 0 0 0 0 0 0

⎞

⎜ ⎟ G6 = O G7 = O G8 = ⎝ 0 1 0 0 0 0 0 0 ⎠ = O G .

(6.119)

0 0 1 0 0 0 0 0 Given M (q) = 3M OTG OG +

8 

Ti iIiG ii ,

(6.120)

˙ i + ii iIiG ii )q, Ti (iIiG i ˙

(6.121)

i

i=6

b(q, q) ˙ =

8 

i

i=6

g(q) = 3MgOTG eˆ3 ,

(6.122)

the nonholonomically constrained equations of motion express as τ = M q¨ + b + g − C T λ,

(6.123)

subject to C q˙ = 0

⇒

C˙ q˙ + C q¨ = 0,

(6.124)

where ⎛

1 0 0 0 −r cos(q4 ) −r sin(q4 ) cos(q5 )

⎜ C = ⎝0 1 0 0

−r sin(q4 )

r cos(q4 ) cos(q5 )

0

0

0 0 1 0

0

0

⎞

⎟ 0 0⎠.

(6.125)

0 0

We note that n = 8 and m = 3. The constrained system has p = n − m = 5 degrees of freedom. The simulation results are displayed in Figures 6.6 and 6.7. All geometric and inertial constants were chosen to be 1. That is, the radius of the ball is 1 and all masses are 1. The initial conditions used were qo = ( −2 q˙o = ( 0

0

0 )T

0.7

1

0

0

0

0

0

0

0

0

0

0 )T .

(6.126) (6.127)

It can easily be confirmed that these initial conditions satisfy the nonholonomic constraints, C q˙ = 0. The applied torques used were τ = ( 0 0 0 0 0 0 0.1

0.275 )T .

(6.128)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

172

First-Order Variational Principles

Figure 6.7 (Top) Time history of the orientation of the ball (α, β, γ ). (Bottom) Time history of the moment wheel angles, q7 and q8 .

Example: We now consider two wheels connected by an axle. We parameterize the system using eight generalized coordinates, as shown in Figure 6.8. The order of rotations of the axle will be specified as a zyx Euler sequence, as used previously: Q = Qz (q4 )Qy (q5 )Qx (q6 ).

O A

(6.129)

The rotation matrices for the two wheels are Q = Qz (q4 )Qy (q5 )Qx (q6 )Qx (q7 )

(6.130)

Q = Qz (q4 )Qy (q5 )Qx (q6 )Qx (q8 ).

(6.131)

O D1 O D2

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

173

6.2 Jourdain’s Principle of Virtual Power

Figure 6.8 Two wheels connected by an axle with nonholonomic constraints. Six generalized

coordinates, q1 , . . . , q6 , parameterize the position of the center of the axle and the axle orientation. Another two generalized coordinates, q7 and q8 , parameterize the rotation of the two wheels. The wheel radii are r, the axle radius is rA , and the axle length is l.

The angular velocity for the axle in the local frame is identical to that of the disk and ball in the previous examples: ⎛ ⎞⎛ ⎞ q˙4 − sin(q5 ) 0 1 ⎜ ⎟⎜ ⎟ A (6.132) ωA = ⎝ cos(q5 ) sin(q6 ) cos(q6 ) 0 ⎠ ⎝ q˙5 ⎠ . cos(q5 ) cos(q6 )

− sin(q6 )

0

q˙6

Given D1

˙ D1 = OD1QT OD1Q,

(6.133)

D2

˙ D2 = OD2QT OD2Q.

(6.134)

The angular velocities of the wheels in the local frames are ⎞ ⎛ − sin(q5 )q˙4 + q˙6 + q˙7 ⎟ ⎜ D1 ⎟ ωD1 = ⎜ ⎝ cos(q5 ) sin(q6 + q7 )q˙4 + cos(q6 + q7 )q˙5 ⎠ cos(q5 ) cos(q6 + q7 )q˙4 − sin(q6 + q7 )q˙5 ⎞ ⎛ − sin(q5 )q˙4 + q˙6 + q˙8 ⎟ ⎜ D2 ⎟ ωD2 = ⎜ ⎝ cos(q5 ) sin(q6 + q8 )q˙4 + cos(q6 + q8 )q˙5 ⎠ cos(q5 ) cos(q6 + q8 )q˙4 − sin(q6 + q8 )q˙5

(6.135)

(6.136)

or, alternatively, D1

ωD1 = Qx (q7 )T AωA + q˙7 eˆ1

(6.137)

D1

ωD2 = Qx (q8 )T AωA + q˙8 eˆ1 .

(6.138)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

174

First-Order Variational Principles

We now express the displacement vectors from the centers of the wheels to the contact points of the wheels with the plane. Since the wheels are always upright in the base frame, we have O

dG−−1−C→1 = OdG−−2−C→2 = −reˆ3 .

(6.139)

The instantaneous velocity of the material point of each wheel in contact with the plane is given by

where

O

vC1 = Ov G1 + (OD1QD1 ωD1 ) × Od−G−1−C→1

(6.140)

O

vC2 = Ov G2 + (OD2QD2 ωD2 ) × Od−G−2−C→2 ,

(6.141)

 l v G1 = v G + Q ωA × eˆ1 , 2   −l O v G2 = Ov G + OAQ AωA × eˆ1 , 2 ⎛ ⎞ q˙1 O v G = ⎝ q˙2 ⎠ . q˙3 

O

O

O A

A

(6.142) (6.143)

(6.144)

Under the condition that the wheels do not slip relative to the plane, we have vC1 = 0

(6.145)

vC2 = 0.

(6.146)

We note that these two vector constraint equations contain a redundant constraint. Specifically, if one wheel is not allowed to move in the axial direction, the other wheel will automatically satisfy this constraint. Expressing the constraints in the axle frame and removing the first component (axial component) from AvC2 yields ⎞ ⎛ A vC1x ⎟ ⎜A ⎜ vC1 ⎟ y ⎟ ⎜ ⎟ ⎜A (6.147) ⎜ vC1z ⎟ = C q˙ = 0, ⎟ ⎜ ⎜ Av ⎟ ⎝ C2y ⎠ A vC2z which are our nonholonomic constraint equations. We now note that v G = OG q, ˙

(6.148)

O

v G1 = OG1 q, ˙

(6.149)

O

v G2 = G2 q, ˙

(6.150)

O

O

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

175

6.2 Jourdain’s Principle of Virtual Power

and that ωA = AA q, ˙

(6.151)

D1

ωD1 = D1 D1 q, ˙

(6.152)

D2

ωD2 = D2 q. ˙

(6.153)

A

D2

Evaluating the Jacobians, we have ⎛ O

⎜ G = ⎜ ⎝0 0 ⎛

O

1

1

⎜ G1 = ⎜ ⎝0 0 ⎛

1 ⎜ O ⎜ G2 = ⎝ 0 0

0

⎞

0

0

0

0

0

0

1

0

0

0

0

0

⎟ 0⎟ ⎠,

0

1

0

0

0

0

0

0

0

− 12 l sin(q4 ) cos(q5 )

1

0

0

1

0

0

1 0

(6.154)

− 12 l cos(q4 ) sin(q5 )

0

0

− 12 l sin(q4 ) sin(q5 )

0

0

0

− 12 l cos(q5 )

0

0

0

1 l sin(q4 ) cos(q5 ) 2 1 − 2 l cos(q4 ) cos(q5 )

1

0

1 l cos(q4 ) sin(q5 ) 2 1 l sin(q4 ) sin(q5 ) 2 1 l cos(q5 ) 2

1 l 2

cos(q4 ) cos(q5 )

0

0

0

0

0

0

0

⎞

⎟ 0⎟ ⎠,

0 (6.155) ⎞ 0 ⎟ 0⎟ ⎠, 0 (6.156)

and ⎛

0

⎜ A = ⎜ ⎝0 0 ⎛ 0 ⎜ D1 ⎜ D1 = ⎝ 0 0 A

⎛

D2

D2

0 ⎜ =⎜ ⎝0 0

⎞

0

0

− sin(q5 )

0

1

0

0

0

cos(q5 ) sin(q6 )

cos(q6 )

0

0

⎟ 0⎟ ⎠,

0

0

cos(q5 ) cos(q6 )

− sin(q6 )

0

0

0

0

0

− sin(q5 )

0

1

1

0

0

cos(q5 ) sin(q6 + q7 )

cos(q6 + q7 )

0

0

0

0

cos(q5 ) cos(q6 + q7 )

− sin(q6 + q7 )

0

0

0

0

− sin(q5 )

0

1

0

0

cos(q5 ) sin(q6 + q8 )

cos(q6 + q8 )

0

0

0

cos(q5 ) cos(q6 + q8 )

− sin(q6 + q8 )

0

0

(6.157)

0

⎞

⎟ 0⎟ ⎠,

0 (6.158) ⎞ 0 1 ⎟ 0 0⎟ ⎠. 0 0 (6.159)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

176

First-Order Variational Principles

We will assume all masses to be the same and take the inertia tensors to be ⎞ ⎛1 2 MrA 0 0 2 ⎟ ⎜ A G 1 1 ⎟ IA = ⎜ MrA2 + 12 Ml 2 0 2 ⎠ ⎝ 0 0 ⎛1 D1

1 MrA2 2

0

⎜ G G ID11 = D2 ID22 = ⎜ ⎝

2

Mr2

+

(6.160)

1 Ml 2 12

⎞

0

0

0

1 Mr2 4

0

0

0

1 Mr2 4

⎟ ⎟, ⎠

(6.161)

where 2 

M = M OTG OG + M

O

TGi OGi + ATA AIAG AA +

i=1

b=M

2 

O

2 

Di

TDi Di IDii Di Di , G

(6.162)

i=1

˙ A + AA AIAG AA )q˙ TGi O˙ Gi q˙ + ATA (AIAG A

i=1

+

2 

Di

˙ D + Di D Di ID i Di D )q, TDi (Di IDii Di  ˙ i i i i G

G

(6.163)

i=1

g = gM OTG eˆ3 + gM

2 

O

TGi eˆ3 .

(6.164)

i=1

Evaluating, we have ⎛ 3M 0 ⎜ ⎜ 0 3M ⎜ ⎜ 0 ⎜ 0 ⎜ ⎜ 0 0 ⎜ M =⎜ ⎜ 0 0 ⎜ ⎜ 0 ⎜ 0 ⎜ ⎜ 0 0 ⎝ 0 0

···

0

0

0

0

3M 0

0 1 M[7l 2 24

+ 9(rA + 2r ) + (7l 2 − 3(rA2 + 2r2 )) cos(2q5 )] 2

2

0

0

0

− 12 M (rA2 + 2r2 ) sin(q5 ) − 12 M (r2 ) sin(q5 ) − 12 M (r2 ) sin(q5 )

0 0 0

0

0

0

0

0

0

0

0

0

0

0 1 M (7l 2 12

+ 3rA + 6r ) 2

0 0 0

2

0

0

1 M (rA2 + r2 ) 2 1 Mr2 2 1 Mr2 2

1 Mr2 2 1 Mr2 2

0

0

⎞

⎟ ⎟ ⎟ ⎟ 0 ⎟ ⎟ 0 ⎟ ⎟ ⎟, (6.165) 0 ⎟ ⎟ 1 2⎟ Mr ⎟ 2 ⎟ 0 ⎟ ⎠ 1 2 Mr 2 0

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

177

6.2 Jourdain’s Principle of Virtual Power

Figure 6.9 (Top) Animation frames from the simulation of the nonholonomic axle and rolling

wheels. (Bottom) Time history of the position of the center of mass of the assembly (x, y, z).

and

⎛

0 0

⎞

⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ 0 ⎜ ⎟ ⎜ 1 ⎟ 2 2 2 2 2 2 ⎜− 6 M cos(q5 )q˙5 [(7l − 3(rA + 2r )) sin(q5 )q˙4 + 3((rA + r )q˙6 + r (q˙7 + q˙8 ))] ⎟ ⎟ b= ⎜ ⎜− 1 M cos(q5 )q˙4 [(7l 2 − 3(r2 + 2r2 )) sin(q5 )q˙4 + 6((r2 + r2 )q˙6 + r2 (q˙7 + q˙8 ))] ⎟, A A ⎜ 12 ⎟ ⎜ ⎟ 1 2 2 ⎜ ⎟ M (r + 2r ) cos(q ) q ˙ q ˙ − 5 4 5 A 2 ⎜ ⎟ ⎜ ⎟ 1 2 − 2 Mr cos(q5 )q˙4 q˙5 ⎝ ⎠ − 12 Mr2 cos(q5 )q˙4 q˙5

(6.166)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

178

First-Order Variational Principles

Figure 6.10 Time history of the wheel angles, q7 and q8 , for the rolling wheel assembly.

and

g= 0

0

3Mg 0

0

0

0

0

T

.

(6.167)

The nonholonomically constrained equations of motion express as τ = M q¨ + b + g − C T λ,

(6.168)

subject to C q˙ = 0

⇒

C˙ q˙ + C q¨ = 0.

(6.169)

We note that n = 8 and m = 5. The constrained system has p = n − m = 3 degrees of freedom (this includes the internal rotation of the axle relative to the wheel hubs). The simulation results are displayed in Figures 6.9 and 6.10. The geometric constants were chosen to be r = 1, rA = 0.075, and l = 2. The masses of all bodies were chosen to be M = 1. No generalized forces were applied (τ = 0). The initial conditions used were qo = ( 2.5 q˙o = ( 0

0 )T

0

1

0

0

0

0

2.5

0

1

0

0

−3.5

(6.170) −1.5 )T .

(6.171)

It can easily be confirmed that these initial conditions satisfy the nonholonomic constraints, C q˙ = 0.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

179

6.3 Kane’s Formulation

6.3

Kane’s Formulation Kane’s approach (Kane 1961) extended Jourdain’s approach to rigid bodies and introduced quasi-velocities to implicitly handle nonholonomic constraints. We will present Kane’s method here in a different way from the original presentation. This will be consistent with the idea of virtual velocities.

6.3.1

A System of Rigid Bodies Given a set of m nonholonomic constraints that are linear in q, ˙ C(q)q˙ = 0,

(6.172)

we can define a set of independent quasi-velocities, s. The generalized velocities are related to these quasi-velocities by the null space of C. That is, q˙ = W (q)s,

(6.173)

im(W ) = ker(C).

(6.174)

where

We note that v(q, q) ˙ = q˙

and

ω(q, q) ˙ = q. ˙

(6.175)

Making use of (6.173), we can replace the q˙ terms in favor of the s terms in our expressions for v and ω. That is, ˜ v˜ (q, s) = W s = s

and

˜ ˜ ω(q, s) = W s = s.

(6.176)

v˜ (q, s) = v(q, q) ˙

and

˜ ω(q, s) = ω(q, q). ˙

(6.177)

Clearly

˜ are the quasi-Jacobians, given by The terms ˜ and  ∂ v˜ = W ˜ = ∂s

and

˜ = ∂ω˜ = W .  ∂s

(6.178)

The virtual velocity variations are given by ˜ + δs) − s ˜ = δs ˜ δ v˜ = (s

(6.179)

˜ + δs) − s ˜ = δs. ˜ δ ω˜ = (s

(6.180)

and

Since the nonholonomic constraint forces vanish under these variations, we have p  i=1

τ˜i · δsi −

nb  i=1

Mi (aGi + geˆ03 ) · δ v˜ Gi −

nb 

(IiGi αi + ωi × IiGi ωi ) · δ ω˜ i = 0

(6.181)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

180

First-Order Variational Principles

for all variations, δ v˜ Gi and δ ω˜ i , that are consistent with the kinematic constraints. The terms τ˜i are the quasi-forces. Expressing the velocity variations in terms of variations in the quasi-velocities (with stationary displacement and time), we have i

δ v˜ Gi = i˜ Gi δs

i

and

˜ i δs, δ ω˜ i = i

(6.182)

where the velocities are expressed in the local link frame i for convenience. So, we have τ˜ · δs −

nb 

Mi ( aGi + g eˆ03 ) · ( ˜ Gi δs) − i

i

i

nb 

i=1

˜ i δs) = 0 (iIiGi iαi + iωi × iIiGi iωi ) · (i

i=1

∀δs ∈ R p , (6.183) or τ˜ · δs −

nb ' 

( ˜ Ti (iIiGi iαi + iωi × iIiGi iωi ) · δs = 0 Mi iTGi iaGi + Mi gi˜ TGi ieˆ03 + i

i=1

(6.184)

∀δs ∈ R . p

In matrix form we have &  % nb    Mi iaGi + Mi gieˆ03 i˜T i ˜ T Gi i τ˜ − · δs = 0 G i Gi i Ii αi + iωi × iIi i iωi i=1

(6.185)

∀δs ∈ R , p

which implies τ˜ =

nb  

i

˜ TGi

i

˜ Ti 



i=1



Mi iaGi + Mi gieˆ03



I αi + iωi × iIi i iωi

i Gi i i

G

.

Replacing the q˙ and q¨ terms in favor of the s and s˙ terms, we have   nb    Mi ia˜Gi + Mi gieˆ03 i˜T i ˜ T Gi i τ˜ = , G i Gi i Ii α˜ i + iω˜ i × iIi i iω˜ i i=1

(6.186)

(6.187)

where a˜Gi (q, s, s) ˙ = aGi (q, q, ˙ q) ¨

and

α˜ i (q, s, s) ˙ = αi (q, q, ˙ q). ¨

(6.188)

Equation (6.187) is a system of p first-order differential equations. We need to complement this with the set of n equations q˙ = W s.

(6.189)

This yields a total system of n + p first-order differential equations in the n + p states, q and s. Noting that i

˜ i s, ω˜ i = i

i

˜ i i˜ G )s a˜Gi = i˜ Gi s˙ + (i˙˜ Gi + i i

and

i

˙˜ s, ˜ i s˙ + i α˜ i = i i

(6.190)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

181

6.3 Kane’s Formulation

Algorithm 7 First-order method for integrating Kane’s equations 1:

q0 = qo {initialization}

2:

s0 = so {initialization}

3:

for i = 0 to ns − 1 do

5:

q˙i = Wi si s˙i = M˜ i−1 (τ˜i − b˜ i − g˜i )

6:

si+1 = si + s˙i t

7:

qi+1 = qi + q˙i t

4:

8:

end for

we have τ˜ =

%n b 

& ˜i I  ˜ i ) s˙ (Mi Gi ˜ Gi +  i

i=1

%

+

˜T

nb  

i

i

Ti

Gi i i

˙˜ + i ˜ i i˜ G ) + i ˜ i iI Gi i ˜ Ti (iIiGi i ˜ i) Mi i˜ TGi (i˙˜ Gi + i i i i

&  s

i=1 nb  Mi i˜ TGi ieˆ03 . +g

(6.191)

i=1

Defining M˜ (q)  ˜ s)  b(q,

nb 

˜ i ), (Mi i˜ TGi i˜ G˜ i + iTi iIiGi i

i=1 %n b  

˙˜ + i ˜ i i˜ G ) + i ˜ i iI G i i ˜ Ti (iIiGi i ˜ i) Mi i˜ TGi (i˙˜ Gi + i i i i

i=1 nb 

g(q) ˜ g

Mi i˜ TGi ieˆ03 ,

(6.192) &  s,

(6.193)

(6.194)

i=1

we have ˜ s) + g(q). τ˜ = M˜ (q)s˙ + b(q, ˜

(6.195)

Numerical Integration The state equation for our system is     Ws q˙ . = ˜ s) − g(q)] s˙ ˜ M˜ (q)−1 [τ˜ − b(q,

(6.196)

A first-order method for integrating this system can be summarized as shown in Algorithm 7.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

182

First-Order Variational Principles

Example: We revisit the rolling disk. As before, we parameterize the system using six generalized coordinates as shown in Figure 6.11. We recall the nonholonomic constraint equations, ⎞ ⎛ q˙1 + r[sin(q4 ) sin(q5 )q˙4 − cos(q4 ) cos(q5 )q˙5 − sin(q4 )q˙6 ] ⎟ ⎜ O vC = ⎝ q˙2 − r[cos(q4 ) sin(q5 )q˙4 + sin(q4 ) cos(q5 )q˙5 − cos(q4 )q˙6 ] ⎠ = 0. (6.197) q˙3 + r sin(q5 )q˙5 We can introduce a set of quasi-velocities s1 = q˙4 ,

s2 = q˙5

and

s3 = q˙6 .

(6.198)

Given the constraint equations, the first three generalized velocities can then be expressed as q˙1 = −r[sin(q4 ) sin(q5 )s1 + cos(q4 ) cos(q5 )s2 + sin(q4 )s3 ],

(6.199)

q˙2 = r[cos(q4 ) sin(q5 )s1 − sin(q4 ) cos(q5 )s2 + cos(q4 )s3 ],

(6.200)

q˙3 = −r sin(q5 )s2 .

(6.201)

So, we have ⎛

−r sin(q4 ) sin(q5 )

⎜ ⎜ r cos(q4 ) sin(q5 ) ⎜ ⎜ 0 ⎜ q˙ = ⎜ ⎜ 1 ⎜ ⎜ 0 ⎝ 0

r cos(q4 ) cos(q5 ) −r sin(q4 ) cos(q5 ) −r sin(q5 ) 0 1 0

r sin(q4 )

⎞

⎟ r cos(q4 ) ⎟ ⎟ ⎟ 0 ⎟ ⎟ s = W s. ⎟ 0 ⎟ ⎟ 0 ⎠

(6.202)

1

We can confirm that im(W ) = ker(C) since CW = 0.

Figure 6.11 A rolling disk with nonholonomic constraints. Six generalized coordinates,

q1 , . . . , q6 , parameterize the position of the center of the disk and the disk orientation. Nonholonomic constraints arise out of the zero velocity (no-slip) condition imposed on the instantaneous contact point of the disk with the ground.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.3 Kane’s Formulation

Evaluating the Jacobians, we have ⎛ −r sin(q4 ) sin(q5 ) ⎜ O˜ O G = GW = ⎝ r cos(q4 ) sin(q5 )

r cos(q4 ) cos(q5 ) −r sin(q5 )

⎛ D

⎞

⎟ −r sin(q4 ) cos(q5 ) r cos(q4 ) ⎠

0 and

r sin(q4 )

− sin(q5 )

(6.203)

0

0

⎜ ˜ = DW = ⎜ cos(q5 ) sin(q6 )  ⎝ cos(q5 ) cos(q6 )

183

1

cos(q6 ) − sin(q6 )

⎞

⎟ 0⎟. ⎠ 0

(6.204)

The dynamical terms are ˜ T DI G D, ˜ M˜ = M O˜ TG O˜ G + D

(6.205)

˙˜ + D ˜ DI G D)]s, ˜ T (D I G D  ˜ b˜ = [M O˜ TG O˙˜ G + D

(6.206)

g˜ = gM O˜ TG eˆ3 .

(6.207)

As before, we will take the inertia tensor to be ⎛1 2 Mr 0 2 ⎜ D G 1 I =⎝ 0 Mr2 4 0 Evaluating, we have ⎛1 ⎜ M˜ = ⎝

8

Mr2 [7 − 5 cos(2q5 )] 0 − 32 Mr2

and

⎛

sin(q5 )

⎞

0

⎟ ⎠.

0

(6.208)

0

1 Mr2 4

0

− 32 Mr2 sin(q5 )

5 Mr2 4

0

0

3 Mr2 2

− 12 Mr2 cos(q5 )s2 [s3 − 5 sin(q5 )s1 ]

⎞ ⎟ ⎠

⎞

⎟ ⎜ b˜ = ⎝ − 14 Mr2 cos(q5 )s1 [−6s3 + 5 sin(q5 )s1 ] ⎠ − 52 Mr2 and

g˜ = 0

(6.209)

(6.210)

cos(q5 )s1 s2

Mgr sin(q5 )

0

T

.

(6.211)

The equations of motion express as τ˜ = M˜ s˙ + b˜ + g. ˜

(6.212)

It can be confirmed that, for the same initial conditions, (6.212) yield the same results as (6.103) and (6.104). In this example, we explore different initial conditions

T

 qo = 1 −0.25 cos π8 (6.213) 0 − π8 0

T (6.214) so = 0.5 0 −2 ,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

184

First-Order Variational Principles

Figure 6.12 (Top) Animation frames from the simulation of the nonholonomic rolling disk using Kane’s method. (Bottom) Time history of the position of the center of mass of the disk (x, y, z).

which correspond to the following conditions on the generalized velocities:

q˙o = W (qo )so = 0

1.809

0

0.5

0

−2

T

.

(6.215)

The simulation results are displayed in Figure 6.12. As before, all geometric and inertial constants were chosen to be 1. That is, r = 1 and M = 1.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.4 Exercises

6.4

185

Exercises 1. Are rolling constraints necessarily nonholonomic? Demonstrate this. 2. Consider the rolling disk shown in Figure 6.13. This is a simplified version of the rolling disk from Section 6.2.5 in that the disk always remains upright (it cannot pitch from side to side). Use a Qz (q4 )Qx (q5 ) Euler sequence to represent the orientation of the disk. The disk radius is r and the mass is M. The principal inertia components are DI11G = 12 Mr2 and DI22G = DI33G = 14 Mr2 . Using Jourdain’s Principle and Lagrange multipliers, answer the following. (a) How many degrees of freedom does the constrained system have? (b) Compute the mass matrix, M (q), for the unconstrained system. (c) Compute the vector of centrifugal and Coriolis forces, b(q, q), ˙ for the unconstrained system. (d) Compute the vector of gravity forces, g(q), for the unconstrained system. (e) Express the velocity condition, OvC , and the constraint matrix, C(q), for the rolling contact.

Figure 6.13 A simplified version of the rolling disk that always remains upright (it cannot pitch

from side to side, Exercises 2 and 3). A Qz (q4 )Qx (q5 ) Euler sequence is used to represent the orientation of the disk. The disk radius is r and the mass is M. The principal inertia components are DI11G = 12 Mr2 and DI22G = DI33G = 14 Mr2 .

3. Consider the rolling disk from Exercise 2 shown in Figure 6.13. Using Kane’s method and defining independent quasi-velocities, s1 = q˙4 and s2 = q˙5 , answer the following. (a) From the rolling contact constraint equations, determine the relationship, q˙ = W s, between the generalized velocities and the quasi-velocities. (b) Compute the mass matrix, M˜ (q). ˜ s). (c) Compute the vector of centrifugal and Coriolis forces, b(q, (d) Compute the vector of gravity forces, g(q). ˜

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

186

First-Order Variational Principles

4. Consider the two-wheeled apparatus shown in Figure 6.14. The apparatus always remains upright (the wheels cannot pitch from side to side). The radius and mass of each wheel are r and M, respectively. The principal inertia components of each wheel are DI22G = 12 Mr2 and DI11G = DI33G = 14 Mr2 . Each link of the connecting linkage has length l, mass M, and center of mass located at the geometric center. Assume the rotational inertia of the connecting links to be zero. Use (q1 , q2 , q3 ) to locate the center of the forward link of the connecting linkage, and use a Qz (q4 )Qy (q5 ) Euler sequence to represent its orientation, as shown. Use q6 for the elbow joint of the connecting linkage, and use q7 and q8 for the wheel angles.

Figure 6.14 (Top) A two-wheeled apparatus (Exercise 4). The apparatus always remains upright

(the wheels cannot pitch from side to side). The radius and mass of each wheel are r and M, respectively. The principal inertia components of each wheel are DI22G = 12 Mr2 and D G I11 = DI33G = 14 Mr2 . Each link of the connecting linkage has length l, mass M, and center of mass located at the geometric center. The rotational inertia of the connecting links are zero. The center of the forward link of the connecting linkage is located by (q1 , q2 , q3 ), and a Qz (q4 )Qy (q5 ) Euler sequence is used to represent its orientation, as shown. The elbow joint of the connecting linkage is q6 , and the wheel angles are q7 and q8 . (Bottom) Animation showing example operation of the two-wheeled apparatus.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

6.4 Exercises

187

Consider the following conditions, q0 = (0 0 0.75 q˙0 = (0.75

0 0

0 0 0 0

π /4

0

0)T ,

− 0.75 sin(π /4)

1

√ 2/2)T .

Using Jourdain’s Principle and Lagrange multipliers, answer the following. (a) How many degrees of freedom does the constrained system have? (b) Compute the mass matrix, M (q), for the unconstrained system at q0 . (c) Compute the vector of centrifugal and Coriolis forces, b(q, q), ˙ for the unconstrained system at q0 and q˙0 . (d) Compute the vector of gravity forces, g(q), for the unconstrained system at q0 . (e) Express the velocity condition, OvC , and the constraint matrix, C(q), for the rolling contact at q0 .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:20:36, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.008

7

Second-Order Variational Principles

In the previous two chapters we addressed zeroth- and first-order variational principles. In this chapter we will focus on the second-order variation of displacement. We will begin with Gauss’s Principle (Gauss 1829). The principle is based on the notion of virtual acceleration. The derivation of the equations of motion for particles and rigid bodies using Gauss’s Principle closely parallels the derivation of the equations of motion using d’Alembert’s Principle and Jourdain’s Principle in the previous chapters. As with d’Alembert’s and Jourdain’s Principles, Gauss’s Principle can be seen as an independent principle of analytical dynamics. Therefore, as with the previous chapters, this chapter is presented in a stand-alone manner.

7.1

Virtual Accelerations Virtual accelerations refer to all accelerations of a system that satisfy the scleronomic constraints. In the case of virtual accelerations, time, position, and velocity are frozen or stationary.

7.2

Gauss’s Principle p r i n c i p l e 7.1 The virtual acceleration work of a system is stationary. That is, δA = 0.

(7.1)

Additionally, the constraints of the system generate no virtual acceleration work, δAc = 0.

(7.2)

This is known as Gauss’s Principle. As with d’Alembert’s Principle and Jourdain’s Principle, it is noted that while Gauss’s Principle can be seen as providing an alternate statement of Newton’s second law, for interacting bodies, a law of action and reaction (Newton’s third law) is still needed. Therefore, when we use Gauss’s Principle to derive the equations of motion for systems of particles/bodies, we will invoke the law of action and reaction.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.2 Gauss’s Principle

7.2.1

189

A Single Particle Gauss’s Principle for a single point mass with a discrete set of n f external forces, { f1 , . . . , fn f }, acting on it is expressed as δA =

nf 

fi · δa − Mgeˆ3 · δa − Ma · δa = 0

i=1

(7.3)

∀δa ∈ R , 3

where δa represents the acceleration variations. During these variations, time, position, and velocity are stationary. That is, δt = 0, δr = 0, and δv = 0. More concisely, we can express   nf  fi − Mgeˆ3 − Ma · δa = 0 δA = (7.4) i=1 ∀δa ∈ R3 , which implies nf 

fi − Mgeˆ3 − Ma = 0.

(7.5)

i=1

7.2.2

A Single Rigid Body Gauss’s Principle for a single rigid body with a discrete set of external forces, fi , and moments, ϕ j , acting on it is expressed as δA =

nf 

fi · δaPi +

i=1

nϕ 

ϕ j · δα − Mgeˆ3 · δaG − Ma · δaG − (I G α + ω × I G ω) · δα = 0,

j=1

∀δaG ∈ R3 ,

and

∀δα ∈ R3 ,

(7.6)

where the δa and δα terms represent all acceleration variations consistent with the rigidbody constraint. This is depicted in Figure 7.1. During these variations, time, position, orientation, and translational and angular velocities are stationary. That is, δt = 0, δr = 0, δθ = 0, δv = 0, and δω = 0. We further note that δaPi = (aG + δaG ) + (α + δα) × d−GP→i + ω × (ω × d−GP→i ) − aG − α × d−GP→i − ω × (ω × d−GP→i ) = δaG + δα × d−GP→i ; then, nf  i=1

fi · δaPi =

nf  i=1

(7.7)

 nf  nf   −→ ) = −→ ). fi · (δaG + δα × dGP f + fi · (δα × dGP · δa i G i i i=1

i=1

(7.8)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

190

Second-Order Variational Principles

Figure 7.1 A virtual acceleration of a rigid body consisting of a translational, δaG , and rotational, δα, acceleration variation. We are concerned with all such acceleration variations consistent with the rigid-body constraint. During these variations, time, position, orientation, and translational and angular velocities are stationary.

Since a · (b × c) = (c × a) · b, nf 

fi · (δα × d−GP→i ) =

i=1

nf 

 (d−GP→i × fi ) · δα =

i=1

So, nf 

nf 

 d−GP→i × fi · δα.

 nf   nf    fi · δaPi = fi · δaG + d−GP→i × fi · δα.

i=1

(7.9)

i=1

i=1

(7.10)

i=1

Substituting (7.10) into (7.6), we get  nf   δA = fi − Mgeˆ3 − Ma · δaG i=1

⎛

+⎝

nf 

d−GP→i × fi +

nϕ 

i=1

or

⎛ ⎜ ⎜ nf ⎝ i=1

⎞ ϕ j − I G α − ω × I G ω⎠ · δα = 0,

(7.11)

j=1

nf

⎞

fi − Mgeˆ3 − Ma

i=1

d−GP→i × fi +

nϕ

ϕ j − I Gα − ω × I Gω

⎟ ⎟· ⎠



δaG δα

 = 0,

(7.12)

j=1

∀δaG ∈ R3 ,

and

∀δα ∈ R3 ,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.2 Gauss’s Principle

191

which implies nf 

fi − Mgeˆ3 − Ma = 0

(7.13)

ϕ j − I G α − ω × I G ω = 0.

(7.14)

i=1 nf 

d−GP→i × fi +

i=1

7.2.3

nϕ  j=1

A System of Particles We now apply Gauss’s Principle to a system of n p particles with a discrete set of forces acting on them. The virtual acceleration work associated with a given particle i is given by ⎛ ⎞ np  fi j ⎠ · δai − Mi (ai + geˆ3 ) · δai = 0 δAi = ⎝ (7.15) j=0 ∀δai ∈ R3 , for i = 1, . . . , n p . The term fi j is the force that particle j exerts on particle i, where fi0 is the force exerted by the ground (inertial reference frame) on the ith particle. Equation (7.15) is true for any particle i in the system, under all independent variations, δai . If we sum (7.15) over all particles, we obtain ⎛ ⎞ np np np np     j⎠ ⎝ δAi = fi · δai − Mi (ai + geˆ3 ) · δai = 0. (7.16) δA = i=1

i=1

j=0

i=1

The first summation is associated with the virtual acceleration work performed by the interparticle reaction forces. It will be useful to rearrange the summation so as to pair up the equal and opposite reaction forces, fi j and f ji . Doing this, we have ⎛ ⎞ np np n p −1 n p np np       j⎠ j i i ⎝ fi · δai = ( fi · δai + f j · δa j ) + fi · δai − f0j · δa0 . i=1

j=0

i=0 j=i+1

i=1

j=1

(7.17) Since fii = 0 and δa0 = 0, we have ⎛ ⎞ np np n p −1 n p     ⎝ fi j ⎠ · δai = ( fi j · δai + f ji · δa j ). i=1

j=0

We note that f ji = − fi j . Thus, ⎛ ⎞ np np n p −1 n p     ⎝ fi j ⎠ · δai = fi j · (δai − δa j ). i=1

j=0

(7.18)

i=0 j=i+1

(7.19)

i=0 j=i+1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

192

Second-Order Variational Principles

This reflects the virtual acceleration work done by interparticle reaction forces, summed over all particles. If we consider that the particles are subject to a set of holonomic constraints, the positions, ri , and in turn velocities, v i , and accelerations, ai , are not independent. In this case we will assume that the accelerations can be expressed in terms of a set of nq independent generalized accelerations, q. ¨ If we now consider only the variations, δai , that are consistent with the kinematic constraints, then (7.19) reflects a projection of the interparticle forces on the direction of the interparticle motion. Gauss’s Principle states that the virtual acceleration work associated with all forces orthogonal to the interparticle motion (reaction forces) is zero (Ac = 0), and only the generalized force acting in the direction of interparticle motion produces virtual acceleration work. Thus, n p −1

np  

fi j · (δai − δa j ) =

nq 

τi · δ q¨i ,

(7.20)

Mi (ai + geˆ3 ) · δai = 0

(7.21)

i=0 j=i+1

i=1

and (7.16) can be expressed as nq 

τi · δ q¨i −

np 

i=1

i=0

for all variations, δv i , that are consistent with the kinematic constraints. Expressing the variations in terms of variations in the generalized velocities (with stationary velocity, displacement, and time), we have ¨ δai = i δ q.

(7.22)

So, (7.21) can be expressed as τ · δ q¨ −

np 

Mi (ai + geˆ3 ) · (i δ q) ¨ =0

i=1

(7.23)

∀δ q¨ ∈ R , n

or

% τ−

np 

& i (Mi ai + Mi geˆ3 ) · δ q¨ = 0 T

i=1

(7.24)

∀δ q¨ ∈ R , n

which implies τ=

np 

Ti (Mi ai + Mi geˆ3 ).

(7.25)

ai = i q, ¨

(7.26)

i=1

Noting that

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.2 Gauss’s Principle

we have τ=

 np 

 Mi i i T

nq  Mi Ti eˆ3 . q¨ + g

i=1

193

(7.27)

i=1

For a system of particles, we had previously defined M (q) 

np 

Mi Ti i

(7.28)

i=1 np

 g(q)  g Mi Ti eˆ3 .

(7.29)

i=1

So, τ = M (q)q¨ + g(q).

7.2.4

(7.30)

A System of Rigid Bodies We now apply Gauss’s Principle to a system of nb rigid bodies forming a serial chain, with a discrete set of forces acting on them. The virtual acceleration work associated with a given body i is given by δAi = fii−1 · δaii−1 + fii+1 · δaii+1 − Mi (aGi + geˆ03 ) · δaGi + ϕii−1 · δαi +ϕii+1 · δαi − (IiGi αi + ωi × IiGi ωi ) · δαi = 0, ∀δaGi ∈ R , 3

and

(7.31)

∀δαi ∈ R , 3

for i = 1, . . . , nb . The term fii−1 is the force that body i − 1 exerts on body i. Likewise, ϕii−1 is the moment that body i − 1 exerts on body i. The term aii−1 is the acceleration of the point on body i to which body i − 1 attaches. Equation (7.31) is true for any rigidbody i in the system, under all independent variations, δaGi and δαi (we note that δaii−1 and δaii+1 are functions of δaGi and δαi due to the rigid body constraint). If we sum (7.31) over all rigid bodies, we obtain δA =

nb 

δAi =

i=1

−

nb  i=1

−

nb 

nb 

( fii−1 · δaii−1 + fii+1 · δaii+1 )

i=1

Mi (aGi + geˆ03 ) · δaGi +

nb 

(ϕii−1 · δαi + ϕii+1 · δαi )

i=1

(IiGi αi + ωi × IiGi ωi ) · δαi = 0.

(7.32)

i=1

The first summation is associated with the virtual acceleration work performed by the interlink reaction forces. It will be useful to rearrange the summation so as to pair up i , acting through each joint i. Doing the equal and opposite reaction forces, fii−1 and fi−1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

194

Second-Order Variational Principles

this, we have nb 

( fi

i−1

· δa

+ fi

i−1 i

i+1

· δa ) = i+1 i

nb 

i=1

i ( fii−1 · δaii−1 + fi−1 · δaii−1 ) + fnnbb +1 · δannbb +1 − f01 · δa10 .

i=1

(7.33) Since fnnbb +1 = 0 and δa10 = 0, we have nb 

( fii−1 · δaii−1 + fii+1 · δaii+1 ) =

i=1

nb 

i ( fii−1 · δaii−1 + fi−1 · δaii−1 ).

(7.34)

i=1

i We note that fi−1 = − fii−1 . Thus,

nb 

( fii−1 · δaii−1 + fii+1 · δaii+1 ) =

nb 

i=1

fii−1 · (δaii−1 − δaii−1 ).

(7.35)

i=1

This reflects the virtual acceleration work done by reaction forces at each joint, summed over all joints. As with the reaction forces, it will be useful to rearrange the summation associated with the virtual acceleration work performed by the interlink reaction moments so as to i , acting about each joint. pair up the equal and opposite reaction moments, ϕii−1 and ϕi−1 Using the same procedure as with the reaction forces, we have nb 

(ϕii−1 · δαi + ϕii+1 · δαi ) =

i=1

nb 

i (ϕii−1 · δαi + ϕi−1 · δαi−1 )

i=1

=

nb 

ϕii−1 · (δαi − δαi−1 ).

(7.36)

i=1

This reflects the virtual acceleration work done by reaction moments at each joint, summed over all joints. So, the total virtual acceleration work performed by the interlink reaction forces and moments can be expressed compactly as nb  i=1

( fii−1 · δaii−1 + fii+1 · δaii+1 ) +

nb 

(ϕii−1 · δαi + ϕii+1 · δαi )

i=1

 nb  i−1    fi δaii−1 − δaii−1 · . = ϕii−1 δαi − δαi−1

(7.37)

i=1

If we now consider only the variations δaii−1 , δaii−1 , δαi , and δαi−1 that are consistent with the kinematic constraints, then (7.37) reflects a projection of the interlink forces and moments on the direction of the joint motion. Gauss’s Principle states that the virtual acceleration work associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Ac = 0) and only the generalized force acting in the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.2 Gauss’s Principle

195

direction of joint motion produces virtual acceleration work. Thus,   nq nb  i−1    fi δaii−1 − δaii−1 · = τi · δ q¨i , ϕii−1 δαi − δαi−1 i=1

(7.38)

i=1

and (7.32) can be expressed as nq 

τi · δ q¨i −

i=1

nb 

Mi (aGi + geˆ03 ) · δaGi −

i=1

nb 

(IiGi αi + ωi × IiGi ωi ) · δαi = 0

(7.39)

i=1

for all variations, δaGi and δαi , that are consistent with the kinematic constraints. Using Gauss’s Principle, we can address a system of nb rigid bodies forming a branching chain in a similar manner. With a serial chain the parent/child structure is implicit to the numbering scheme. Every link i has a single parent link, i − 1, at its proximal end and a single child link, i + 1, at its distal end (except for the nth link). The ith joint is at the proximal end of the ith link. With a branching chain, the numbering of links is more arbitrary, without a parent/child structure implicit to the numbering scheme. We can explicitly capture the parent/child structure, however, by defining three additional parameters, λi , ci , μi j . The term λi is the parent link number of the ith link, ci is the number of child links for the ith link, and μi1 . . . μici are the child link numbers of the ith link. Given these parameters, the virtual acceleration work associated with a given body i is given by δAi = fiλi · δaλi i +

ci 

fi

μi j

μ

· δai i j − Mi (aGi + geˆ03 ) · δaGi + ϕiλi · δαi

j=1

+

ci 

(7.40)

μ

ϕi i j · δαi − (IiGi αi + ωi × IiGi ωi ) · δαi = 0,

j=1

∀δaGi ∈ R3 ,

∀δαi ∈ R3 ,

and

λ

for i = 1, . . . , nb . The term ai i is the acceleration of the point on body i to which body μic μ (parent) λi attaches, and likewise ai i1 . . . ai i are the accelerations of points on body i μic μ λ to which bodies (children) μi1 . . . μici attach. We note that δai i and δai i1 . . . δai i are functions of δaGi and δαi due to the rigid-body constraint. If we sum (7.40) over all rigid bodies, we obtain ⎛ ⎞ nb nb ci    μ μ ⎝ fiλi · δaλi i + δAi = fi i j · δai i j ⎠ δA = i=1

−

nb  i=1

−

nb 

i=1

j=1

Mi (aGi + geˆ03 ) · δaGi +

nb 

⎛ ⎝ϕiλi · δαi +

i=1

(IiGi αi + ωi × IiGi ωi ) · δαi = 0.

ci 

⎞ ϕi · δαi ⎠ μi j

j=1

(7.41)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

196

Second-Order Variational Principles

The term associated with the virtual acceleration work performed by the interlink reaction forces is ⎛ ⎞ nb ci   μ μ ⎝ fiλi · δaλi i + fi i j · δai i j ⎠. (7.42) i=1

j=1

It will be useful to rearrange the summation so as to pair up the equal and opposite λ reaction forces, fi i and fλii , acting through each joint i. We can rewrite the summation of (7.42) based on considering the virtual acceleration work at each joint. The sum over all joints then gives us nb 

( fiλi · δaλi i + fλii · δaiλi ) =

i=1

nb 

fiλi · (δaλi i − δaiλi ).

(7.43)

i=1

The term associated with the virtual acceleration work performed by the interlink reaction moments is ⎛ ⎞ nb ci   μ ⎝ϕiλi · δαi + ϕi i j · δαi ⎠. (7.44) i=1

j=1

In a similar manner as before, we can rewrite this summation based on considering the virtual acceleration work at each joint. The sum over all joints then gives us nb 

(ϕiλi · δαi + ϕλi i · δαλi ) =

i=1

nb 

ϕiλi · (δαi − δαλi ).

(7.45)

i=1

So, the total virtual acceleration work performed by the interlink reaction forces and moments can be expressed compactly as ⎛ ⎞ ⎛ ⎞ nb ci nb ci     μ μ μ i j i j i j ⎝ fiλi · δaλi i + ⎝ϕiλi · δαi + fi · δai ⎠ + ϕi · δαi ⎠ i=1

j=1

i=1

j=1

 nb  λi   λ  δai i − δaiλi fi · . = λ ϕi i δαi − δαλi

(7.46)

i=1

λ

If we now consider only the variations δai i , δaiλi , δαi , and δαλi that are consistent with the kinematic constraints, then (7.46) reflects a projection of the interlink forces and moments on the direction of the joint motion. Gauss’s Principle states that the virtual acceleration work associated with all forces and moments orthogonal to the joint motion (reaction forces/moments) is zero (Ac = 0) and only the generalized force acting in the direction of joint motion produces virtual acceleration work. Thus,   nq nb  λi   λ  δai i − δaiλi fi τi · δ q¨i , · = λi ϕi δαi − δαλi i=1

(7.47)

i=1

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

197

7.2 Gauss’s Principle

and (7.41) can be expressed as nq 

τi · δ q¨i −

i=1

nb 

Mi (aGi + geˆ03 ) · δaGi −

nb 

i=1

(IiGi αi + ωi × IiGi ωi ) · δαi = 0

(7.48)

i=1

for all variations, δaGi and δαi , that are consistent with the kinematic constraints. Expressing the variations in terms of variations in the generalized accelerations (with stationary velocity, displacement, and time), we have δaGi = iGi δ q˙

i

i

and

δαi = ii δ q, ˙

(7.49)

where the accelerations are expressed in the local link frame i for convenience. So, (7.48) can be expressed as τ · δ q¨ −

nb 

Mi ( aGi + g eˆ03 ) · ( Gi δ q) ¨ − i

i

i

i=1

nb 

(iIiGi iαi + iωi × iIiGi iωi ) · (ii δ q) ¨ =0

i=1 n

∀δ q¨ ∈ R , (7.50) or τ · δ q¨ −

nb ' 

( Mi iTGi iaGi + Mi giTGi ieˆ03 + iTi (iIiGi iαi + iωi × iIiGi iωi ) · δ q¨ = 0

i=1

(7.51)

∀δ q¨ ∈ R . n

In matrix form we have % &  nb    Mi iaGi + Mi gieˆ03 i T i T Gi i τ− · δ q¨ = 0 G i Gi i Ii αi + iωi × iIi i iωi i=1

(7.52)

∀δ q¨ ∈ R , n

which implies τ=

nb  

Gi i

i

T

i

T

i=1





Mi iaGi + Mi gieˆ03 I αi + ii iIi i iωi

i Gi i i

G

 .

(7.53)

Since this is identical to (5.53), we have τ = M (q)q¨ + b(q, q) ˙ + g(q).

7.2.5

(7.54)

Auxiliary Constraints Second-Order Nonholonomic Constraints Second-order nonholonomic constraints are nonintegrable functions of the generalized coordinates, generalized velocities, and generalized accelerations (Udwadia and Kalaba 1992). We will address these constraints here.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

198

Second-Order Variational Principles

We now consider the general case of auxiliary second-order nonholonomic constraints. Given the multibody system, τ = M (q)q¨ + b(q, q) ˙ + g(q),

(7.55)

subject to the second-order nonholonomic constraints A(q, q) ˙ q¨ + ψ(q, q) ˙ = 0.

(7.56)

We begin by first expressing the second-order variational equation associated with Gauss’s Principle, τC · δ q¨ + (τ − M q¨ − b − g) · δ q¨ = 0.

(7.57)

The virtual accelerations, δ q, ¨ refer to all acceleration variations that satisfy the constraints, while time, displacement, and velocity are fixed. With δt = 0, δq = 0, and δ q˙ = 0, the second-order variation of the constraint equation yields Aδ q¨ = 0,

(7.58)

which implies that δ q¨ ∈ ker(A). Under this condition, (7.57) can be restricted to constraint-consistent virtual displacements, τC · δ q¨ + (τ − Mq − b − g) · δ q¨ = 0 ∀δ q¨ ∈ ker(A).

(7.59)

We have τC ⊥δ q¨ ∀δ q¨ ∈ ker(A).

(7.60)

Thus the term τC · δ q¨ vanishes from (7.59), and we have the orthogonality relation (Mq + b + g − τ) · δ q¨ = 0 ∀δ q¨ ∈ ker(A).

(7.61)

The constrained multibody equations of motion, expressed in the familiar multiplier form, are thus τ = M q¨ + b + g − AT λ,

(7.62)

Aq¨ + ψ = 0.

(7.63)

subject to

In the special case of first-order nonholonomic constraints, ψ(q, q) ˙ = 0,

(7.64)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.2 Gauss’s Principle

199

we can differentiate once with respect to time to yield ∂ψ ∂ψ q˙ + q¨ = 0. ψ˙ = ∂q ∂ q˙

(7.65)

The second-order variation is given by δ ψ˙ =

∂ψ δ q¨ = δ q¨ = 0, ∂ q˙

(7.66)

and the constrained multibody equations of motion express identically to the first-order case, τ = M q¨ + b + g − T λ,

(7.67)

subject to ψ(q, q) ˙ =0

⇒

ψ˙ = 0.

(7.68)

In the special case of first-order nonholonomic constraints that are linear in q, ˙ ψ(q, q) ˙ = C(q)q˙ + d(q) = 0,

(7.69)

we can differentiate once with respect to time to yield ψ˙ = C q¨ + C˙ q˙ + d˙ = 0.

(7.70)

The second-order variation is given by δ ψ˙ = Cδ q¨ = 0,

(7.71)

and the constrained multibody equations of motion express identically to the first-order case, τ = M q¨ + b + g − C T λ,

(7.72)

subject to C q˙ + d = 0

⇒

C q¨ + C˙ q˙ + d˙ = 0.

(7.73)

In the special case of zeroth-order holonomic constraints, φ(q) = 0,

(7.74)

we can differentiate twice with respect to time to yield ˙ q˙ = 0. φ¨ = q¨ + 

(7.75)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

200

Second-Order Variational Principles

Algorithm 8 Second-order method for integrating the second-order nonholonomically constrained equations of motion 1:

q0 = qo {initialization}

2:

q˙0 = q˙o {initialization}

for i = 0 to ns − 1 do 4: q¨i = −A¯ i ψi + i Mi−1 (τi − bi − gi ) 5: λi = −A¯ T (τi − bi − gi ) − Hi ψi 3:

i

6:

q˙i+1 = q˙i + q¨i t

7:

qi+1 = qi + q˙i t + 12 q¨i t 2

8:

end for

The second-order variation is given by δ φ¨ = δ q¨ = 0,

(7.76)

and the constrained multibody equations of motion express identically to the zerothorder case, τ = M q¨ + b + g − T λ,

(7.77)

subject to φ(q) = 0

⇒

φ˙ = 0, φ¨ = 0.

(7.78)

Numerical Integration Given the nonholonomic system τ = M q¨ + b + g − AT λ,

(7.79)

Aq¨ + ψ = 0,

(7.80)

    q¨ τ −b−g = , λ ψ

(7.81)

subject to

the equations of motion are 

M −AT −A 0

and the solution of this system is ¯ + M −1 (τ − b − g) q¨ = −Aψ λ = −A¯ T (τ − b − g) − Hψ,

(7.82) (7.83)

¯ and H = (AM −1 AT )−1 . where = 1 − AA A second-order method for integrating this system can be summarized as shown in Algorithm 8.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

201

7.3 Gauss’s Principle of Least Constraint

7.3

Gauss’s Principle of Least Constraint Gauss’s Principle of Least Constraint uses the second variation to express the dynamics of a system as a minimization problem. The Principle of Least Constraint is a differential local minimum principle, as opposed to Hamilton’s Principle, which is an integral extremal principle. In the next section we will apply Gauss’s Principle of Least Constraint to rigid-body systems.

7.3.1

A System of Rigid Bodies We begin by noting the second-order orthogonality relation for a multibody system, (Mq + b + g − τ) · δ q¨ = 0 ∀δ q¨ ∈ ker(A),

(7.84)

subject to the constraints A(q, q) ˙ q¨ + ψ(q, q) ˙ = 0.

(7.85)

If we define the quadratic Gauss function, G, as G(q) ¨ =

1 (M q¨ + b + g − τ)T M −1 (M q¨ + b + g − τ), 2

(7.86)

the variation, δG, with respect to the virtual accelerations is δG =

∂G · δ q¨ = (M q¨ + b + g − τ) · δ q. ¨ ∂ q¨

(7.87)

Thus, we can write (7.84) as δG = 0 ∀δ q¨ ∈ ker(A).

(7.88)

This implies a constrained minimization of G(q), ¨ subject to A(q, q) ˙ q¨ + ψ(q, q) ˙ = 0.

(7.89)

Since we can express G as G=

1 1 T −1 τC M τC = τC 2M −1 , 2 2

(7.90)

this constrained minimization seeks a constraint-consistent generalized acceleration that minimizes the M −1 -weighted norm of the generalized constraint forces, τC . Alternatively, we can write G as G=

T   1 q¨ + M −1 (b + g − τ) M q¨ + M −1 (b + g − τ) . 2

(7.91)

Noting that the unconstrained generalized acceleration, q¨ , is q¨ = M −1 (τ − b − g),

(7.92)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

202

Second-Order Variational Principles

Figure 7.2 The Gauss function, G, is minimized subject to the constraints. At the solution point, q, ¨ the gradient of G is orthogonal to the space of virtual accelerations. Geometrically, the solution minimizes the distance (mass weighted) to the unconstrained acceleration, q¨ .

we can write G as 1 1 (7.93) (q¨ − q¨ )T M (q¨ − q¨ ) = q¨ − q¨ 2M . 2 2 Thus, the constrained minimization of G also seeks a constraint-consistent generalized acceleration that minimizes the M-weighted norm of the difference between the actual constrained generalized acceleration and the unconstrained generalized acceleration. This is illustrated in Figure 7.2. G(q) ¨ =

7.3.2

Hertz’s Principle of Least Curvature Using Gauss’s Principle, we can examine the special case of a particle moving in n dimensions, r(t ) = q(t ) ∈ Rn , under holonomic constraints but no applied force. That is, M r¨ = T λ subject to φ(r) = 0. For this system we have the Gauss function 1 M (¨r − r¨ )T (¨r − r¨ ). 2 In the absence of applied forces, r¨ = 0, so G=

(7.94)

1 1 T M r¨ r¨ = M ¨r, r¨ . (7.95) 2 2 The vector r can be parameterized in terms of arc length, s, in which case we have the following relationships: G=

r˙ =

dr dr ds = , dt ds dt

(7.96)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.3 Gauss’s Principle of Least Constraint

and r¨ =

d dr d 2r = 2 dt dt ds



ds dt

2 +

dr d 2 s . ds dt 2

The Gauss function can then be expressed as %     -  2 2 & , 2 d 2 r 2 ds 4 dr 2 d 2 s 2 ds d s r 1 d dr G= M + +2 , . 2 2 2 2 ds dt ds dt ds ds dt dt 2 We note that

and

203

(7.97)

(7.98)

2 , dr = dr , dr = 1 ds ds ds

(7.99)

, , 2 d dr dr d r dr , =0=2 . , ds ds ds ds2 ds

(7.100)

Thus,

%    2 2 & d 2 r 2 ds 4 1 d s G= M + . 2 2 ds dt dt 2 Noting that the extrinsic path curvature is k = d 2 r/ds2 , we have %    2 2 & 4 1 ds d s G = M k2 + , 2 dt dt 2

(7.101)

(7.102)

subject to the constraints. The term ds/dt is the particle speed, determined from the system state, and d 2 s/dt 2 is the tangential acceleration. Both k 2 and (d 2 s/dt 2 )2 are positive numbers. Additionally, they can be selected (and minimized) independently of each other. Therefore, minimizing G subject to the constraints implies that k 2 (and k) is minimized subject to the constraints and that d 2 s/dt 2 = 0. The implication that curvature is minimized reflects Hertz’s Principle of Least Curvature (Hertz 1894; Lanczos 1986; Papastavridis 2002), which states that under force-free constrained motion, a system will follow the path of least extrinsic curvature, k, on the constrained-motion manifold, Q p . Furthermore, this constrained minimization implies ∇k 2 ⊥Tr (Q p ), where Tr (Q p ) denotes the tangent space of Q p at the point r. We note that 2 2 d r d2r 2 ∇k = ∇ (7.103) ds2 = 2 ds2 , so d 2r ⊥Tr (Q p ). ds2 This implies that the covariant derivative, D/ds, of the path tangent vanishes,   2   d dr d r D dr = 0, = projT = projT ds ds ds ds ds2

(7.104)

(7.105)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

204

Second-Order Variational Principles

Figure 7.3 Geodesic force-free paths on a constrained-motion manifold. Force-free motion

minimizes the extrinsic path curvature, k, subject to the constraints, yielding zero geodesic (intrinsic) curvature, kg .

where projT () denotes the projection of a vector onto the tangent space of Q p . The intrinsic geodesic curvature of the path, D dr , (7.106) kg = ds ds is thus zero. This implies that under force-free constrained motion, a system will follow geodesics (paths for which kg = 0) on the constrained-motion manifold. This is illustrated in Figure 7.3. Since a geodesic path minimizes arc length the condition of zero geodesic curvature is equivalent to finding a path which minimizes the action defined in terms of arc length. That is,

δI = δ ds = 0, (7.107) subject to the constraints. Equivalently, for a system with no external forces this fact can be concluded from Jacobi’s form of least action (Goldstein et al. 2002), which states,

t f T dt = 0,

δI = δ

(7.108)

to

subject to the constraints. In this case the Lagrangian has been replaced by the kinetic energy alone (no potential energy). Since  (7.109) dt = M/2T ds,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.3 Gauss’s Principle of Least Constraint

we have

 δI = δ MT /2 ds = 0.

205

(7.110)

Because T is constant for this system, (7.110) implies that arc length is minimized on the constrained-motion manifold. One application of the least curvature principle involves computing the geodesics of a surface by solving an analogous mechanical system. By the Principle of Least Curvature, solutions of this system will trace out geodesics on the constrained-motion surface Q p ⊂ R3 . In three dimensions, geodesics can be computed by solving the set of second-order nonlinear differential equations (2.153), given in Chapter 2. Alternatively, we can solve the analogous mechanical system, which, by the Principle of Least Curvature, will trace out geodesics on the constrained-motion surface Q p ⊂ R3 . This system is M r¨ = T λ

subject to, φ(r) = 0,

(7.111)

where φ(r) = 0 is an implicit representation of the surface represented parametrically by r(u, v). Using the acceleration form of the constraint equations, the system of (7.111) can be solved to yield the differential equations ˙ r, r¨ = −T (T )−1 ˙

(7.112)

or, incorporating constraint stabilization, ˙ r + β˙r + αφ). r¨ = −T (T )−1 (˙

(7.113)

Thus, (7.113) represents a mechanically derived approach for computing geodesics. Equation (7.113) is not limited to R3 and can be used to compute geodesics in Rn .

Example: We can apply (7.113) to the problem of computing geodesics for the surface: φ(x, y, z) = −

4 sin(4x) sin(4y) + 7 x2 + y2 + − z = 0. 2 10

The constraint Jacobian is computed directly from φ as

  = ∂φ/∂x ∂φ/∂y ∂φ/∂z ,

(7.114)

(7.115)

and (7.113) yields a system of three second-order nonlinear differential equations in x, y, and z: ˙ r + β˙r + αφ). ( x¨ y¨ z¨ )T = −T (T )−1 (˙

(7.116)

Specifying the point (xo, yo, zo ) = (0.175, −0.175, 0.503) as one initial condition, we can solve (7.116) using different departure directions, (x˙o, y˙o, z˙o ). The resulting geodesics are shown in Figure 7.4. It is noted that the specific time parameterization used does not affect the shape of the paths, only the speed at which they are traversed.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

206

Second-Order Variational Principles

Figure 7.4  the system of (7.113), geodesic force-free paths were computed for the surface

Solving

φ = − x2 + y2 /2 + [sin(4x) sin(4y) + 7]/10 − z = 0. All paths were chosen to emanate from the point (0.175, −0.175, 0.503).

Therefore, only the direction (not the magnitude) of the initial velocity dictates the path.

7.4

Gibbs-Appell Formulation The Gibbs-Appell formulation is a second-order variational method that introduces quasi-velocities and quasi-accelerations to implicitly handle nonholonomic constraints in much the same way that Kane’s method introduces quasi-velocities. The formulation proceeds using virtual variations on the accelerations rather than virtual variations on velocities. As we shall see, since the Gibbs-Appell equations are derived from a second-order variational principle, they can be stated as the gradient of a scalar function with respect to the quasi-accelerations. In the next section we will formulate the Gibbs-Appell equations for rigid-body systems.

7.4.1

A System of Rigid Bodies Given a set of m nonholonomic constraints described by C(q)q˙ = 0,

(7.117)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.4 Gibbs-Appell Formulation

207

we can define a set of independent quasi-velocities, s. The generalized velocities are related to these quasi-velocities by the null space of C. That is, q˙ = W (q)s,

(7.118)

im(W ) = ker(C).

(7.119)

where

We note that v(q, q) ˙ = q˙

and

ω(q, q) ˙ = q. ˙

(7.120)

Making use of (7.118), we can replace the q˙ terms in favor of the s terms in our expressions for v and ω. That is, ˜ v˜ (q, s) = W s = s

and

˜ ˜ ω(q, s) = W s = s.

(7.121)

Differentiating (7.118), we have ˙ s. q¨ = W s˙ + W

(7.122)

The term, s, ˙ is the vector of quasi-accelerations. We note that a(q, q, ˙ q) ¨ = q¨ + (˙ + )q˙

and

˙ q. α(q, q, ˙ q) ¨ = q¨ +  ˙

(7.123)

Making use of (7.118) and (7.122), we can replace the q˙ and q¨ terms in favor of the s and s˙ terms in our expressions for a and α. That is, ˜ ˙ ) + W ]s = ˜ s˙ + (˙˜ + )s ˙ + W a(q, ˜ s, s) ˙ = W s˙ + [(W

(7.124)

and ˙˜ ˙ )s =  ˜ s˙ + s, ˙ + W ˜ s, s) α(q, ˙ = W s˙ + (W

(7.125)

where p = n − m. Clearly v˜ (q, s) = v(q, q) ˙

and

˜ ω(q, s) = ω(q, q), ˙

(7.126)

a(q, ˜ s, s) ˙ = a(q, q, ˙ q) ˙

and

˜ s, s) α(q, ˙ = α(q, q, ˙ q). ¨

(7.127)

and

˜ are the quasi-Jacobians given by The terms ˜ and  ∂ a˜ = W ˜ = ∂ s˙

and

˜ ˜ = ∂α = W .  ∂ s˙

(7.128)

The virtual acceleration variations are then given by ˜ s˙ + δ s) ˜ − ˜ s˙ − (˙˜ + )s ˜ = δ ˜ s˙ δ a˜ = ( ˙ + (˙˜ + )s

(7.129)

˙˜ −  ˙˜ = δ ˜ s˙ + δ s) ˜ s˙ − s ˜ s. δ α˜ = ( ˙ + s ˙

(7.130)

and

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

208

Second-Order Variational Principles

Since the nonholonomic constraint forces vanish under these variations, we have p 

nb 

τ˜i · δ s˙i −

i=1

nb 

Mi (aGi + geˆ03 ) · δ a˜Gi −

i=1

(IiGi αi + ωi × IiGi ωi ) · δ α˜ i = 0

(7.131)

i=1

for all variations, δ a˜Gi and δ α˜ i , that are consistent with the kinematic constraints. The terms τ˜i are the quasi-forces. Expressing the acceleration variations in terms of variations in the quasi-accelerations (with stationary displacement and time), we have δ a˜Gi = i˜ Gi δ s˙

i

i

and

˜ i δ q, δ α˜ i = i ˙

(7.132)

where the accelerations are expressed in the local link frame i for convenience. So, we have τ˜ · δ s˙ −

nb 

Mi ( aGi + g eˆ03 ) · ( ˜ Gi δ s) ˙ − i

i

i

nb 

i=1

˜ i δ s) (iIiGi iαi + iωi × iIiGi iωi ) · (i ˙ =0

i=1

∀δ s˙ ∈ R p , (7.133) or nb ' (  ˜ Ti (iIiGi iαi + iωi × iIiGi iωi ) · δ s˙ = 0 Mi iTGi iaGi + Mi gi˜ TGi ieˆ03 + i τ˜ · δ s˙ − i=1

(7.134)

∀δ s˙ ∈ R . p

In matrix form we have % nb   i˜T G τ˜ −

i

i

˜i 

T



i=1



Mi iaGi + Mi gieˆ03

& · δ s˙ = 0,

I αi + iωi × iIi i iωi ,

i Gi i i

G

(7.135)

∀δ s˙ ∈ R p which implies τ˜ =

nb   i=1

i

˜ TGi

i

˜ Ti 





Mi iaGi + Mi gieˆ03 I αi + iωi × iIi i iωi

i Gi i i

G

 .

Replacing the q˙ and q¨ terms in favor of the s and s˙ terms, we have   nb    Mi ia˜Gi + Mi gieˆ03 i ˜ T i˜T Gi i τ˜ = . G i Gi i Ii α˜ i + iω˜ i × iIi i iω˜ i i=1

(7.136)

(7.137)

Equation (7.137) is a system of p first-order differential equations. We need to complement this with the set of n equations q˙ = W s.

(7.138)

This yields a total system of n + p first-order differential equations in the n + p states, q and s. Noting that i

˜ i s, ω˜ i = i

i

˜ i i˜ G )s a˜Gi = i˜ Gi s˙ + (i˙˜ Gi + i i

and

i

˙˜ s, ˜ i s˙ + i α˜ i = i i

(7.139)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.4 Gibbs-Appell Formulation

we have τ˜ =

%n b 

209

& ˜i I  ˜ i ) s˙ (Mi Gi ˜ Gi +  i

˜T

Ti

i

i

Gi i i

i=1

%

& nb    i˜T i˙ i ˜ i˜ i ˜ T i Gi i ˙ i ˜ i Gi i ˜ ˜ Mi Gi ( Gi + i Gi ) + i ( Ii i + i Ii i ) s + i=1 nb  Mi i˜ TGi ieˆ03 . +g

(7.140)

i=1

As with Kane’s method, we have ˜ s) + g(q). τ˜ = M˜ (q)s˙ + b(q, ˜

(7.141)

Now let us define the Gibbs function, S, b 1 [Mi ia˜TGi ia˜Gi + 2ia˜TGi Mi gieˆ03 + iα˜ Ti iIiGi iα˜ i + 2iα˜ Ti (iω˜ i × iIiGi iω˜ i )]. 2 i=1

n

S (s) ˙ =

(7.142)

Recognizing that ∂ a˜Gi = ˜ Gi ∂s˙ we observe that the gradient of S is b   ∂S i˜T Gi = ∂ s˙ i=1

n

i

∂ α˜ i ˜ i, = ∂s˙

and

˜i 

T





Mi ia˜Gi + Mi gieˆ03 G I α˜ i + iω˜ i × iIi i iω˜ i

i Gi i i

(7.143)  ˜ = τ.

(7.144)

So, ∂S ∂ s˙

(7.145)

q˙ = W s

(7.146)

τ˜ = and

form the Gibbs-Appell dynamical equations. We can relate the Gibbs function to the Gauss function by first noting that the Gauss function can be written as %n  &  b    Mi iaGi + Mi gieˆ03 i i T T Gi i δG = − τ · δ q. ¨ (7.147) G i Gi i Ii αi + iωi × iIi i iωi i=1 Replacing the q˙ and q¨ terms in favor of the s and s˙ terms, we have  & %n  b   Mi ia˜Gi + Mi gieˆ03  i i T T Gi i − τ˜ · W δ s˙ δ G˜ = G i Gi i Ii α˜ i + iω˜ i × iIi i iω˜ i i=1  & %n  b    Mi ia˜Gi + Mi gieˆ03 T i T i T Gi i W − τ · δ s, ˙ = G i Gi i Ii α˜ i + iω˜ i × iIi i iω˜ i i=1

(7.148)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

210

Second-Order Variational Principles

and finally, δ G˜ =

%n b  

i

˜ TGi

i

˜ Ti 





i=1

Mi ia˜Gi + Mi gieˆ03 G i Gi i Ii α˜ i + iω˜ i × iIi i iω˜ i

Noting that

%n b   ∂S i˜T Gi · δ s˙ = δS = ∂ s˙ i=1

˜ Ti 

i





& · δ s˙ − τ˜ · δ s. ˙

Mi ia˜Gi + Mi gieˆ03 G G i Ii i iα˜ i + iω˜ i × iIi i iω˜ i

(7.149)

& · δ s, ˙

(7.150)

we have δS = δ G˜ + τ˜ · δ s. ˙

(7.151)

Example: Recalling the rolling disk from Chapter 6 (see Figure 7.5), the Gibbs function can be formulated as 1 ˜ (7.152) S = [M Oa˜TG Oa˜G + 2Oa˜TG Mgeˆ3 + Dα˜ T DI G Dα˜ D + 2Dα˜ T (Dω˜ × DI G Dω)]. 2 The translational and angular velocities can be expressed in terms of the quasivelocities, s: ⎛ ⎞ r[cos(q4 ) cos(q5 )s2 + sin(q4 )(s3 − s1 sin(q5 ))] ⎜ ⎟ O v˜ G = O˜ G s = ⎝ r[− cos(q4 )s3 + cos(q5 )s2 sin(q4 ) + cos(q4 )s1 sin(q5 )] ⎠ (7.153) −rs2 sin(q5 ) ⎞ s3 − s1 sin(q5 ) ⎟ D ˜ =⎜ ω˜ = Ds ⎝ cos(q6 )s2 + cos(q5 )s1 sin(q6 ) ⎠ , ⎛

(7.154)

cos(q5 ) cos(q6 )s1 − s2 sin(q6 )

Figure 7.5 A rolling disk with nonholonomic constraints. Six generalized coordinates, q1 , . . . , q6 , parameterize the position of the center of the disk and the disk orientation. Nonholonomic constraints arise out of the zero velocity (no-slip) condition imposed on the instantaneous contact point of the disk with the ground.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

211

7.5 Exercises

and the translational and angular accelerations can be expressed in terms of the quasiaccelerations, s, ˙ as a˜G = Ov˙˜ G |q→s ˙ D D˙ ˜ q→s α˜ = ω| . ˙

O

(7.155) (7.156)

We can now derive the Gibbs function: S=

1 Mr 8rs42 + 8rs41 sin2 (q5) − rs31 s3 [17 sin(q5 ) + sin(3q5 )] + 7rs˙21 16 + s22 [−16g cos(q5 ) + r(25 + 11 cos(2q5 ))s21 + 6rs23 − 12rs1 s3 sin(q5 )] − 5r cos(2q5 )s˙21 + 24r cos(q5 )s1 s3 s˙2 − 16g sin(q5 )s˙2 + 10rs˙22 + rs21 [(11 + 3 cos(2q5 ))s23 − 10 sin(2q5 )s˙2 ] − 24r sin(q5 )s˙1 s˙3 + 12rs˙23 − 8r cos(q5 )s2 [s3 s˙1 + 5s1 (− sin(q5 )s˙1 + s˙3 )]) .

(7.157)

The gradient of S is then ∂S = ∂ s˙

1 8



Mr2 [−4 cos(q5 )s2 (s3 − 5s1 sin(q5 )) + (7 − 5 cos(2q5 ))s˙1 − 12 sin(q5 )s˙3 ] 1 Mr[12r cos(q5 )s1 s3 − 8g sin(q5 ) − 5Rs21 sin(2q5 ) + 10rs˙2 ] . 8 − 12 Mr2 [5 cos(q5 )s1 s2 + 3 sin(q5 )s˙1 − 3s˙3 ]

(7.158)

It can be verified that this is equal to the expression ˜ s) + g(q), M˜ (q)s˙ + b(q, ˜

(7.159)

generated using Kane’s method in the example of Section 6.3.1.

7.5

Exercises 1. Consider the planar four-bar linkage from Section 5.6, Exercise 3 (shown in Figure 7.6). The link lengths are each l = 1 and the link masses are each M = 1. The link rotational inertias are taken to be zero: Let τ = 0 and,

qo = −2π /3

2π /3

π /3

T

and

q˙o = 1

−1

1

T

.

(a) If not already completed, compute the configuration space mass matrix, M (q), the vector of centrifugal and Coriolis forces, b(q, q), ˙ and the vector of gravity forces, g(q), for the unconstrained system. (b) If not already completed, compute the constraint matrix, (q), for the loop closure. (c) Compute the Gauss function, G(q), ¨ at qo and q˙o. (d) Compute the gradient, ∂G/∂ q. ¨ (e) Determine a basis for ker() at qo and q˙o.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

212

Second-Order Variational Principles

Figure 7.6 A planar four-bar linkage (Exercise 1). The unconstrained system is parameterized by

the generalized coordinates, q1 , q2 , and q3 , with generalized forces, τ1 , τ2 , and τ3 . The link lengths are each l and the link masses are each M. The centers of mass are at the geometric centers of the links. The link rotational inertias are taken to be zero. The link 3 endpoint is constrained from translating.



Figure 7.7 Implicit surface φ = 4 + x2 + y2 − z −

1 3

cos(3x) cos(3y)

2

= 0 (Exercise 2).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

7.5 Exercises

213

Figure 7.8 A simplified version of the rolling disk (Gibbs-Appell method) that always remains

upright (it cannot pitch from side to side, Exercise 3). A Qz (q4 )Qx (q5 ) Euler sequence is used to represent the orientation of the disk. The disk radius is r and the mass is M. The principal inertia components are DI11G = 12 Mr2 and DI22G = DI33G = 14 Mr2 .

(f) Use Gauss’s Principle of Least Constraint to solve for the constrained accelerations, q, ¨ at qo and q˙o. You will have two scalar equations in q¨ by taking the second derivative of φ(q) = 0 and one scalar equation from the orthogonality relation, δG =

∂G = 0. ∂ q¨

2. Numerically generate geodesics for the surface (see Figure 7.7) 2  1 2 2 φ = 4 + x + y − z − cos(3x) cos(3y) = 0 3 in the region x, y = [−1, 1] by interpreting the problem in terms of constrained dynamics. Choose several departure directions emanating from the point (0.25, −0.25, −1.85255) to generate the geodesics. 3. Consider the rolling disk from Section 6.4, Exercise 3 shown in Figure 7.8. Use the Gibbs-Appell method and define independent quasi-velocities, s1 = q˙4 and s2 = q˙5 . (a) If not already completed, from the rolling contact constraint equations, determine the relationship, q˙ = W s, between the generalized velocities and the quasivelocities. (b) Derive the Gibbs function, S. (c) Compute the gradient of the Gibbs function, ∂S/∂ s, ˙ to obtain the equations of motion.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:16:54, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.009

8

Dynamics in Task Space

In the previous chapters we considered configuration space descriptions of the dynamics of constrained multibody systems. In this chapter our objective is to reformulate these descriptions in the context of task space. As a starting point, we begin with a review of the basic operational space framework developed by Khatib (1987, 1995). Operational space is synonymous with task space. It refers to a motion space that is a mapping from configuration space to a convenient coordinate space with which to formulate the system dynamics.

8.1

Task Space Framework The basic task space framework addresses the dynamics of branching chain mechanical systems. Given a branching chain system, the initial step involves defining a set of mT task, or task space, coordinates, x. The function x(q) represents a kinematic mapping from the set of generalized coordinates to the set of task space coordinates. The task space coordinates can represent any function of the generalized coordinates but typically are chosen to describe the set of coordinates associated with a motion task. Figure 8.1 illustrates simple branching chain systems where the task space coordinates are chosen to be the coordinates associated with positioning the terminal point(s) of the chain. Furthermore, by taking the gradient of x, we have the relationship x˙ =

∂x q˙ = J (q)q, ˙ ∂q

(8.1)

where J (q) is the mT × n task Jacobian matrix. This relationship applies to both kinematically nonredundant and redundant systems. Kinematically redundant systems are those where the number of degrees of freedom exceed, the number of task coordinates (mT < n), resulting in an infinite number of configurations for a given task. In the case of nonredundant systems the inverse of this relationship is well defined outside of singularities. For such cases we have ˙ q˙ = J −1 x.

(8.2)

In the redundant case we can define the right inverse of J as {J # |JJ # = 1}.

(8.3)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.1 Task Space Framework

215

Figure 8.1 Task descriptions for serial and branching chains. (Left) A simple task description for

a serial chain where the task space coordinates describe the Cartesian position of the terminal point of the chain. (Right) A branching chain where the task space coordinates describe the positions of both terminal points.

The solutions to the underdetermined equation, J q˙ = x, ˙ are thus given by q˙ = J # x˙ + N q˙o,

(8.4)

where N  1 − J J and q˙o is an arbitrary vector in R . We now define a matrix, W , whose columns span the null space of J. This implies that im(W ) = ker(J ). Thus, JW = 0 and W T J T = 0. In this manner, W orthogonally complements J T . That is, #

n

im(W ) = ker(J ) = im(J T )⊥ .

(8.5)

If we form the projection matrix, P = P T , that projects any vector in Rn onto the null space of J, we have P = W W T = 1 − J + J = 1 − J T (JJ T )−1 J,

(8.6)

and a specific solution of the inverse kinematics problem, using the pseudoinverse J + , is q˙ = J + x˙ + P q˙o = J + x˙ + (1 − J + J )q˙o.

(8.7)

This can be used for kinematic path generation given a task trajectory, x(t ). If some minimization criterion, U (q), is specified we have, q˙ = J + x˙ − P

∂U . ∂q

(8.8)

At this point we can address task space kinetics. In the nonredundant case, any generalized force can be produced by a task space force, f , acting at the task point along the task coordinates. Figure 8.1 illustrates the action of the task space force for the intuitive case of Cartesian positioning tasks. The generalized force is then composed as J T f , by the Principle of Virtual Work (3.184). In the redundant case an additional term needs

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

216

Dynamics in Task Space

to complement the task term to realize any arbitrary generalized force. We will refer to this term as the null space term, and it can be composed as N T τo, where N T is the null space projection matrix. An arbitrary generalized force, τ, can be expressed as τ = J T f + N T τo = M q¨ + b + g.

(8.9)

We can premultiply (8.9) by JM −1 and rearrange to get x ¨ = JM −1 J T f + JM −1 N T τo − JM −1 b − JM −1 g + J˙q, ˙

(8.10)

where we note that x ¨ = J q¨ + J˙q. ˙ We can now impose the condition that the term associated with the null space, N T τo, does not contribute to the task space acceleration. This is referred to as dynamic consistency (Khatib 1995) and is expressed as JM −1 N T τo = JM −1 (1 − J T J T # )τo = 0 ∀τo ∈ Rn .

(8.11)

¯ the dynamically We can solve for J # under this condition and denote this solution as J, consistent inverse of J (Khatib 1995): J¯ = M −1 J T (JM −1 J T )−1 .

(8.12)

This represents a unique right inverse of J where, by construction, the null space projection matrix, N T = 1 − J T J¯T , will not influence the task acceleration. The matrix N T can be regarded as a mass-weighted null space projection matrix, compared to P T = W W T , which can be regarded as a kinematic null space projection matrix. We can manipulate (8.10) to arrive at f = (JM −1 J T )−1 x ¨ + (JM −1 J T )−1 (JM −1 b − J˙q) ˙ + (JM −1 J T )−1 JM −1 g.

(8.13)

This expresses the task space dynamical equation f = (q)¨ x + μ(q, q) ˙ + p(q),

(8.14)

where (q) is the mT × mT symmetric positive definite task space mass matrix, μ(q, q) ˙ is the mT × 1 task space centrifugal and Coriolis force vector, and p(q) is the mT × 1 task space gravity force vector: (q) = (JM −1 J T )−1 , ˙ − J˙q, ˙ μ(q, q) ˙ = J¯T b(q, q) p(q) = J¯T g(q), J¯T = JM −1 .

(8.15) (8.16) (8.17) (8.18)

Thus, the overall dynamics of our multibody system can be mapped into task space using J¯T : J¯T

τ = M q¨ + b + g → f = ¨ x + μ + p.

(8.19)

In a complementary manner the overall dynamics can be mapped into the task-consistent null space (or self-motion space) using N T . The null space term is guaranteed not to

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.1 Task Space Framework

217

interfere with the task dynamics of (8.14) due to the condition of dynamic consistency. Finally, the overall torque applied to the system is composed as in (8.9).

Example: We consider the serial chain robot of Section 5.2.4 (see Figure 8.2). We can use the configuration space properties already computed to formulate the task space dynamics. We will specify the task to be the position of the distal end of link 4 (the terminal point in the chain denoted by rE ).  x = rE = 0

l1 cos(q1 ) + l2 cos(q1 + q2 ) + l3 cos(q1 + q2 + q3 ) l1 sin(q1 ) + l2 sin(q1 + q2 ) + l3 sin(q1 + q2 + q3 ) ···

+l4 cos(q1 + q2 + q3 + q4 ) +l4 sin(q1 + q2 + q3 + q4 )

 .

(8.20)

The task Jacobian is, J (q) =

∂x . ∂q

(8.21)

Using J (q), as well as M (q), b(q, q), ˙ and g(q), which were computed in the example of Section 5.2.4, we can compute the task space terms, (q) = (JM −1 J T )−1 , ˙ − J˙q, ˙ μ(q, q) ˙ = JM −1 b(q, q) p(q) = JM −1 g(q).

(8.22) (8.23) (8.24)

Figure 8.2 A 4 degree-of-freedom serial chain robot parameterized using four generalized

coordinates. The task point, x, is shown.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

218

Dynamics in Task Space

Figure 8.3 (Top) Animation frames from the simulation of a serial chain robot. (Bottom) Time history of the robot generalized coordinates (q1 , q2 , q3 , q4 ).

Since these terms involve matrix inverses it is preferable to compute them numerically at each time step within the numerical integration loop, rather then to compute them symbolically. Our task space equation of motion for the dynamics at the task point is, f = (q)¨ x + μ(q, q) ˙ + p(q).

(8.25)

We will specify a task space force at the task point to cancel out the task space centrifugal and Coriolis force, and the task space gravity force. That is, f = μ(q, q) ˙ + p(q).

(8.26)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.1 Task Space Framework

219

Figure 8.4 Time history of the robot task space gravity vector.

This will result in the following dynamics, (q)¨ x = 0.

(8.27)

Therefore, the task point will have zero acceleration. Given zero velocity initial conditions, the task point will not move. The redundant chain will however move in the null space. The simulation results are displayed in Figures 8.3 and 8.4. The following values were used for the initial conditions, T

π (8.28) qo = − 12 − π8 − π6 − π4 ,

T q˙o = 0 0 0 0 . (8.29)

8.1.1

Inertial Properties The task space kinetic energy is given by, T =

1 T ˙ x˙ x. 2

(8.30)

We will assume this expression is represented in the base frame for convenience. Since  is symmetric positive definite it has positive eigenvalues and an orthogonal eigenbasis (principal axes), E. Therefore, T =

1 1O TO O x˙  x˙ = E x˙ T OEQT O OEQE x, ˙ 2 2

(8.31)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

220

Dynamics in Task Space

where the columns of OEQ are the eigenvectors, {v 1 , v 2 , v 3 }, of O, ⎛ ⎞ ↑ ↑ ↑ ⎜ ⎟ O Q = ⎝v 1 v 2 v 3 ⎠. E ↓

↓

(8.32)

↓

We then have, QT O OEQ = E ,

O E

where E  is a diagonal matrix of eigenvalues, {λ1 , λ2 , λ3 }, of O, ⎞ ⎛ λ1 0 0 ⎟ ⎜ E  = ⎝ 0 λ2 0 ⎠. 0

0

(8.33)

(8.34)

λ3

In terms of the eigenbasis, 1E TE E x˙  x. ˙ (8.35) 2 For constant values of the kinetic energy T , this represents an ellipsoid expressed with respect to the principal axes. In scalar form, T =

 1 E 2 λ1 x˙ 1 + λ2 E x˙ 22 + λ3 E x˙ 32 . 2 The semiaxes of the ellipsoid in frame E are then,  ai = 2T /λi . T =

(8.36)

(8.37)

Khatib introduced the concept of the belted ellipsoid as another means of visualizing task space inertial properties (Khatib 1995). The belted ellipsoid is given by the following equation, v T −1 v = 1. v

(8.38)

This representation inverts the task space mass matrix in the quadratic form and normalizes by v. The points on the belted ellipsoid correspond to the effective masses/inertias in different unit directions.

Example: We consider the serial chain robot of Section 4.1.4 (see Figure 8.5). We can use the equations of motion already computed in Section 4.1.4 to form M (q), b(q, q), ˙ and g(q). This will allow us to formulate the task space dynamics. We will specify the task to be the position of the distal end of link 3 (the terminal point in the chain denoted by rE ). ⎞ ⎛ cos(q1 )[l1 + l2 cos(q2 ) + l3 cos(q2 + q3 )] ⎟ ⎜ ⎟ x = 0r E = ⎜ (8.39) ⎝ [l1 + l2 cos(q2 ) + l3 cos(q2 + q3 )] sin(q1 ) ⎠ . l2 sin(q2 ) + l3 sin(q2 + q3 )

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

221

8.1 Task Space Framework

Figure 8.5 A 3 degree-of-freedom serial chain robot parameterized by the generalized

coordinates, q1 , q2 , and q3 . The link lengths are l1 , l2 , and l3 , the radii are r1 , r2 , and r3 , and the masses of the links are M1 , M2 , and M3 . The task point, x, is shown.

The task Jacobian is, ⎛

−[l1 + l2 cos(q2 ) + l3 cos(q2 + q3 )] sin(q1 ) ⎜ ⎜ J (q) = ⎝ [l1 + l2 cos(q2 ) + l3 cos(q2 + q3 )] cos(q1 ) 0 − cos(q1 )[l2 sin(q2 ) + l3 sin(q2 + q3 )] · · · − sin(q1 )[l2 sin(q2 ) + l3 sin(q2 + q3 )] l2 cos(q2 ) + l3 cos(q2 + q3 ) −l3 cos(q1 ) sin(q2 + q3 )]

⎞

⎟ · · · −l3 sin(q1 ) sin(q2 + q3 )] ⎟ ⎠. l3 cos(q2 + q3 ) Using J (q) and M (q), we can compute the task space mass matrix, (q) = (JM −1 J T )−1 . As noted in the previous example, since this term involves matrix inverses it is preferable to compute it numerically rather then symbolically. Using the following values for the constants, r1 = 0.125, r2 = 0.1, r3 = 0.09, l1 = 1,

l2 = 0.75, l3 = 0.5,

(8.40)

M1 = 1, M2 = 0.75, M3 = 0.5,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

222

Dynamics in Task Space

Figure 8.6 Kinetic energy ellipsoids for a sequence of configurations.

we can evaluate (q) for an arbitrary configuration. At q1 = −π /2, q2 = π /3, q3 = π /3 we have, ⎛ ⎞ 1.732 0 0 ⎜ ⎟ (−π /2, π /3, π /3) = ⎝ 0 (8.41) 0.391 0.336⎠ , 0 and, solving the eigenvalue problem yields, ⎛ ⎞ ⎛ ↑ ↑ ↑ 1 ⎜ ⎟ ⎜ O Q = ⎝v 1 v 2 v 3 ⎠ = ⎝0 E ↓ and,

⎛ E

λ1

↓

0

↓

0

⎞

0 ⎛

0.336

0

λ2

⎟ ⎜ 0⎠=⎝ 0

0

0

λ3

0.833

0

⎞

0

⎟ 0.833 ⎠,

0.553

1.732

⎜  =⎝0

0.674

−0.553

0 0.898 0

(8.42)

0

⎞

⎟ 0 ⎠.

(8.43)

0.168

Figure 8.6 displays kinetic energy ellipsoids for a sequence of configurations. The belted ellipsoids, given by, v T −1 v = 1, v

(8.44)

for a sequence of configurations are shown in Figure 8.7.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.1 Task Space Framework

223

Figure 8.7 Belted ellipsoids for a sequence of configurations.

8.1.2

Energy Minimization Minimization of Gravitational Potential Energy Given the self-motion manifold associated with a fixed task point, xO , M (xO ) = {q|x(q) = xO },

(8.45)

there is a configuration, qo, which minimizes the potential energy, V (q), on M (xO ). This configuration satisfies, ∇V |qo ∈ Tqo (M )⊥ = ker(J )⊥ |qo = im(J T )|qo .

(8.46)

g(qo ) = ∇V |qo = J T , (qo )c

(8.47)

c = J¯T (qo )g(qo ) = p(qo ),

(8.48)

g(qo ) = J T (qo )p(qo ).

(8.49)

This implies,

for some vector, c. Thus,

so,

That is, under static equilibrium the configuration associated with the minimum potential energy on the self-motion manifold for a fixed task point corresponds to a force of p(qo ) acting at the task point.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

224

Dynamics in Task Space

Minimization of Kinetic Energy We can seek a solution to the kinematic relationship, J q˙ = x, ˙ which minimizes the kinetic energy, T =

1 T q˙ M q. ˙ 2

(8.50)

The solution to this constrained minimization problem is straightforward and yields, ˙ q˙ = M −1 J T (JM −1 J T )−1 x.

(8.51)

Noting that the dynamically consistent inverse of J is given by, J¯ = M −1 J T (JM −1 J T )−1 ,

(8.52)

we have that q˙ = J¯x˙ yields the kinetic energy minimizing solution of (8.1). Similarly, we can seek a solution of the acceleration expression, J q¨ = x ¨ − J˙q, ˙

(8.53)

which minimizes the acceleration energy, defined as the following mass weighted quadratic form, 1 T q¨ M q. ¨ 2

(8.54)

x − J˙q), ˙ q¨ = M −1 J T (JM −1 J T )−1 (¨

(8.55)

This yields,

and we have that q¨ = J¯(¨ x − J˙q) ˙ yields the acceleration energy minimizing solution of (8.53).

8.1.3

Task and Constraint Symmetry There are parallels between the structure of the constrained multibody dynamics problem, in both the multiplier and minimization (Gauss) forms, and the task space formulation. These parallels are derived from the common mathematical description used for tasks and constraints. Both utilize a Jacobian representation (constraint matrix, , or task Jacobian, J) and tasks can be viewed as rheonomic constraints of the form x(q) = xd (t ). Despite the common form used in specifying them, the mechanism by which constraints and tasks are satisfied differs. Constraints are imposed by the physical structure of the multibody system, whereas tasks are achieved by means of an applied control input. Nevertheless, due to their common mathematical form there are similarities between the structure of task dynamics and constrained dynamics.

Symmetry Between Projection Matrices The generalized constrained equation of motion (5.154) provides a unique perspective into constrained dynamics. The projection matrix, T , filters out the component of the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.1 Task Space Framework

225

generalized force which acts in the direction of the constraint force. That is, ¯ T τ.

T τ = τ −  T 

(8.56)

Consequently, only the component of the generalized force which influences the motion of the system is preserved. Equivalent motion (Huston, Liu, and Li 2003) is produced by all choices of τ which differ by a vector lying in the im(T ). We also note that the complementary spaces defined by im(T ) and im( T ) are orthogonal in a mass weighted sense, * T +  , T M −1 = M −1 T = 0. (8.57) Similarly, in task space, the projection matrix, N T , filters out the component of the generalized force which produces acceleration in the task direction. That is, N T τ = τ − J T J¯T τ.

(8.58)

Consequently, only the component of the generalized force which influences the internal self-motion of the system is preserved. We also note that the complementary spaces defined by im(J T ) and im(N T ) are orthogonal in a mass weighted sense, * T T+ J , N M −1 = JM −1 N T = 0. (8.59) To summarize, in the case of constrained motion, a projection matrix, T , is used to project the overall system dynamics into the constraint null space. This preserves only the dynamics which influences the constrained motion of the system. In the case of task space dynamics a projection matrix, N T , is used to project the overall system dynamics into the task null space. This preserves only the dynamics which influences the task-consistent self-motion of the system.

Energy Minimization ¯ of the task We observed in Section 8.1.2 that the dynamically consistent inverse, J, Jacobian provides the unique task-consistent solution of q¨ which minimizes both the kinetic energy and acceleration energy of the system. This is analogous to the manner ¯ of the constraint matrix yields a constraintin which the mass weighted inverse, , consistent solution that minimizes the Gauss function. That is, q¨ = J¯(¨ x − J˙q) ˙

(8.60)

J q¨ = x ¨ − J˙q, ˙

(8.61)

1 q ¨ 2M , 2

(8.62)

¯ ˙ q˙ + (1 − ) ¯ q¨ q¨ = −

(8.63)

is the solution of

which minimizes

and

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

226

Dynamics in Task Space

is the solution of ˙ q, q¨ = − ˙

(8.64)

which minimizes G=

1 q¨ − q¨ 2M . 2

(8.65)

Least Action in Task Space It was noted earlier that tasks can be viewed as rheonomic constraints. We will now interpret the task space representation as a rheonomically constrained least action problem. Noting that δxd (t ) = 0, since time is fixed under all virtual variations, we have δx = Jδq = 0. Application of the Principle of Least Action then yields δI = 0, ∀δ|δq(to ) = δq(t f ) = 0, and Jδq = 0.

(8.66)

Thus,

t f  δI = to

∂L d ∂L − ∂q dt ∂ q˙

 · δqdt = 0

(8.67)

∀δq|Jδq = 0. This implies the following Euler-Lagrange equations: d ∂L ∂L − = JT f . dt ∂ q˙ ∂q

(8.68)

Identical equations could have been obtained by embedding the rheonomic constraints directly in the Lagrangian. Adjoining the constraints yields Laug (q, q, ˙ λ)  L(q, q) ˙ + f T [x(q) − xd (t )],

(8.69)

and the stationary value of I would be sought for all variations that vanish at the endpoints. Recalling that δxd = 0, δLaug = δL + f T δx = δL + f T Jδq.

(8.70)

In either case, (8.68) can be expressed as J T f = M q¨ + b + g,

(8.71)

subject to x(q) ¨ = x¨ d (t ). This is similar to (8.9), except that no null space term is present. That is, τ = J T f (absent any null space torque) achieves the task space

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.2 Constrained Dynamics in Task Space

227

Figure 8.8 Task description for a multibody system with loop constraints. The task coordinates,

x, can be assigned to a point on one of the links. More generally, the task can be defined as any function of the generalized coordinates that does not violate the constraints.

objective, consistent with least action. Projecting (8.71) into task space yields the task space equations f = x¨ + μ + p.

8.2

(8.72)

Constrained Dynamics in Task Space We will now present task space methodologies for addressing constrained systems in the task space framework. As an example, Figure 8.8 illustrates a task description for a multibody system with loop constraints. The task coordinates, x, are assigned to a point on one of the links in a manner that does not violate the constraints. In the following sections we will formalize approaches for representing constrained system dynamics in a task space form.

8.2.1

Direct Task Space Mapping This formulation involves directly mapping the generalized constrained equation of motion (5.154) into task space coordinates using the dynamically consistent inverse of the task Jacobian. Alternatively, we will make use of the constrained equation of motion in which the Lagrange multipliers have been eliminated through the introduction of a minimal set of independent coordinates (5.302). As with (5.154), this equation can be directly mapped into task space coordinates using the dynamically consistent inverse of the task Jacobian.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

228

Dynamics in Task Space

We begin by recalling the generalized constrained equation of motion (5.154)

T τ = M q¨ + b + g − T (ρ + α).

(8.73)

We can relate a set of task coordinates, x, to the set of generalized coordinates, q, by x˙ = J q. ˙

(8.74)

Mapping (8.73) into any appropriate task space via the dynamically consistent inverse of J yields x + μ(q, q) ˙ + p(q) + γ (q), J¯T T τ = (q)¨

(8.75)

(q) = (JM −1 J T )−1 ,

(8.76)

where

μ(q, q) ˙ = J¯T b − J˙q, ˙ p(q) = J¯T g,

(8.78)

γ = −J¯T T (p + α), J = JM ¯T

(8.77)

−1

.

(8.79) (8.80)

The procedure applied to (5.154) can also be applied to (5.302). In this case, however, the task space mapping is associated with the minimal set of generalized coordinates, q p . We begin by recalling (5.302): ˙ q˙ p ) + g p (q). τ p = M p (q)q¨ p + b p (q, q,

(8.81)

We can relate a set of task coordinates, x, to the coordinates, q p , by x˙ = J q˙ = Jp (q)q˙ p ,

(8.82)

Jp (q) = JW

(8.83)

where

is the task Jacobian with respect to the minimal set of coordinates. We can map (8.81) into any appropriate task space via the dynamically consistent inverse of Jp . This yields (Russakow et al. 1995) f = (q)¨ x + μ(q, q, ˙ q˙ p ) + p(q),

(8.84)

where (q) = (Jp M p−1 JpT )−1 ,

(8.85)

μ(q, q, ˙ q˙ p ) = J¯pT b p − J˙p q˙ p ,

(8.86)

p(q) = J¯pT g p , J¯pT = Jp M p−1 .

(8.87) (8.88)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.2 Constrained Dynamics in Task Space

229

We can express (8.84) in terms of the generalized forces: x + μ + p. J¯pT τ p = J¯pT W T τ = ¨

8.2.2

(8.89)

Task/Constraint Partitioning of Dynamics In this formulation the multiplier form of the constrained equation of motion (5.86) is mapped into task space using the dynamically consistent inverse of a Jacobian that characterizes both task and constraints (De Sapio and Khatib 2005; De Sapio, Khatib, and Delp 2006). The resulting task space equation is then partitioned into an equation corresponding to task motion and an equation corresponding to constraint forces. We begin by recalling the multiplier form of the constrained equation of motion (5.86): τ = M q¨ + b + g − T λ.

(8.90)

Again, we can relate a set of task coordinates, x, to the set of generalized coordinates, q, by x˙ = J q, ˙

(8.91)

φ˙ = q˙ = 0.

(8.92)

in addition to the constraint condition

We can concatenate (8.91) and (8.92) into a single vector:     x˙ J ˙ xˇ = ˙ = q˙ = Jˇq, ˙ φ 

(8.93)

ˇ to represent a quantity that is formed from the composition where we use the notation  of task and constraint terms. The applied generalized force can be decomposed into a task space component and null space component as in (8.9):    f

+ Nˇ T τo, (8.94) τ = JˇT fˇ + Nˇ T τo = J T T fC where fC is the component of the applied task space force acting along the constraint direction. Equation (8.90) can thus be written as     f 0 + Nˇ T τo = M q¨ + b + g − JˇT . (8.95) JˇT fC λ We can now map (8.95) into task space by premultiplying (8.95) by the dynamically T consistent inverse, J¯ˇ , of JˇT . This yields 

f fC



    x ¨ 0 ˇ ˇ = (q) + μ(q, q) ˙ + p(q) ˇ − , 0 λ

(8.96)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

230

Dynamics in Task Space

where the constraint condition, φ¨ = 0, has been imposed. We have the following identities:  ˇ (q) =

JM −1 J T

JM −1 T

M −1 J T M −1 T   JM −1 b − J˙q˙ ˇ ˇ μ(q, q) ˙ = , ˙ q˙ M −1 b −    JM −1 g ˇ . p(q) ˇ = M −1 g

−1 ,

(8.97)

(8.98)

(8.99)

While (8.96) expresses the combined task-constraint dynamics of our system, it is useful to partition the dynamics in the following manner: 

f fC



 =

ˇ 11  ˇ 21 

ˇ 12  ˇ 22 

        x ¨ μˇ 1 pˇ 1 0 + − + . μˇ 2 pˇ 2 0 λ

(8.100)

From this partitioning we have an equation corresponding to task motion, ˇ 11 x ¨ + μˇ 1 + pˇ 1 , f =

(8.101)

and an equation corresponding to the constraint forces, ˇ 21 x ¨ + μˇ 2 + pˇ 2 . fC + λ = 

(8.102)

The constraint force vector, λ, will always arise so as to satisfy (8.102), as dictated by constraint consistency. The component of applied task space force, fC , acting along the constraint direction has no impact on the motion of the task. Its only effect is on the constraint forces (values of λ) that arise. Thus, all choices for fC result in equivalent motion of the system (Huston et al. 2003). Example: We consider the parallel mechanism of Section 5.2.5 (see Figure 8.9). We can use the configuration space properties already computed to formulate the task space dynamics. We will specify the task to be the center point, rG p , of the platform

x = 0rG p = q7

q8

T

.

(8.103)

The task Jacobian is   0 0 0 0 0 0 1 0 0 ∂x = . J (q) = ∂q 0 0 0 0 0 0 0 1 0

(8.104)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.2 Constrained Dynamics in Task Space

231

Figure 8.9 Parallel mechanism consisting of serial chains with loop closures. The three elbow joints are actuated while the remaining joints are passive. The task point, x, is shown.

Using J (q), as well as (q), M (q), b(q, q), ˙ and g(q), which were computed in the example of Section 5.2.5, we can compute the task/constraint space terms: −1  JM −1 J T JM −1 T ˇ , (8.105) (q) = M −1 J T M −1 T   JM −1 b − J˙q˙ ˇ ˇ μ(q, q) ˙ = , (8.106) ˙ q˙ M −1 b −    JM −1 g ˇ p(q) ˇ = . (8.107) M −1 g As in the previous examples, since these terms involve matrix inverses, it is preferable to compute them numerically at each time step within the numerical integration loop rather then to compute them symbolically. Our task/constraint space equation of motion for the dynamics at the task point is ˇ 11 x ¨ + μˇ 1 + pˇ 1 , f =

(8.108)

and the equation corresponding to constraint forces in the system is ˇ 21 x ¨ + μˇ 2 + pˇ 2 . fC + λ = 

(8.109)

We will specify a task space force at the task point to cancel out the task/constraint space centrifugal and Coriolis force and the task/constraint space gravity force. That is, f = μˇ 1 + pˇ 1 .

(8.110)

This will result in the following dynamics: ˇ 11 x ¨ = 0. 

(8.111)

Therefore, the task point will have zero acceleration. Given zero velocity initial conditions, the task point will not move. The system will, however, move in the null space.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

232

Dynamics in Task Space

Figure 8.10 (Top) Animation frames from the simulation of a parallel mechanism. (Bottom) Time history of the platform motion.

Results from simulation of the system are shown in Figure 8.10. The initial conditions used were as follows:

T qo = π /4 π /6 11π /12 π /6 −5π /12 π /6 0 2.165 0 (8.112)

T q˙o = 3.331 −6.57 3.331 −6.57 3.331 −6.57 0 0 π /2 . (8.113) It can be verified that these satisfy the constraints.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

8.3 Exercises

8.3

233

Exercises 1. Consider the serial chain robot from Section 5.6, Exercise 1 (shown in Figure 8.11). The link lengths are l1 = 0.85 and l2 = 0.5. The link radii are r1 = 0.125 and r2 = 0.0875, the link masses are M1 = 1 and M2 = 1.5, and the link inertia tensors are ⎞ ⎛ 1 M r2 0 0 2 i i ⎟ ⎜ i Gi 1 2 ⎟ Ii = ⎜ l Mi + 14 Mi ri2 0 12 i ⎠ ⎝ 0 1 2 1 2 l Mi + 4 Mi ri 0 0 12 i for links i = 1, 2. The task, x, is defined as the x and y components of the endpoint of link 2. Let

T

T qo = π /4 π /3 and q˙o = 1 2 .

Figure 8.11 A 2 degree-of-freedom serial chain robot parameterized by the generalized

coordinates, q1 and q2 , with generalized forces, τ1 and τ2 (Exercise 1). The link lengths are l1 = 0.85 and l2 = 0.5. The centers of mass are at the geometric centers of the links. The link radii are r1 = 0.125 and r2 = 0.0875, the link masses are M1 = 1 and M2 = 1.5, and the link G G G principal inertia components are iIi11i = 12 Mi ri2 and iIi22i = iIi33i = 121 li2 Mi + 14 Mi ri2 for links i = 1, 2. The task, x, is defined as the x and y components of the endpoint of link 2.

(a) If not already completed, compute the configuration space mass matrix, M (q), the vector of centrifugal and Coriolis forces, b(q, q), ˙ and the vector of gravity forces, g(q). (b) Express the task vector, x(q), and the task Jacobian, J (q). (c) Compute the task space mass matrix, (q), at qo. (d) Find the eigenvalues and eigenvectors of (q) at qo and specify the kinetic energy ellipsoid. (e) Compute the task space vector of centrifugal and Coriolis forces, μ(q, q), ˙ at qo and q˙o. (f) Compute the task space vector of gravity forces, p(q), at qo. 2. Consider the two-link planar slider-crank mechanism from Section 5.6, Exercise 2 shown in Figure 8.12. The link lengths are each l = 1 and the link masses are each

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

234

Dynamics in Task Space

Figure 8.12 A two-link planar slider-crank mechanism (Exercise 2). The unconstrained system is

parameterized by the generalized coordinates, q1 and q2 , with generalized forces, τ1 and τ2 . The link lengths are each l = 1 and the link masses are each M = 1. The centers of mass are at the geometric centers of the links. The link rotational inertias are taken to be zero. Link 2 is constrained from translating in the yˆ direction. The task, x, is defined as q1 .

M = 1. The link rotational inertias are taken to be zero. The task, x, is defined as q1 . Let √ T

T

√ and q˙o = qo = −π /6 2π /6 3/2 − 3 . (a) If not already completed, compute the configuration space mass matrix, M (q), the vector of centrifugal and Coriolis forces, b(q, q), ˙ and the vector of gravity forces, g(q), for the unconstrained system. (b) If not already completed, compute the constraint matrix, (q), for the loop closure. (c) Express the task vector, x(q), and the task Jacobian, J (q). (d) Compute the task space mass matrix, (q), at qo for the unconstrained system. (e) Compute the task space vector of centrifugal and Coriolis forces, μ(q, q), ˙ at qo and q˙o for the unconstrained system. (f) Compute the task space vector of gravity forces, p(q), at qo for the the unconstrained system. ˇ (g) Compute the task/constraint space mass matrix, (q), at qo for the constrained system. (h) Compute the task/constraint space vector of centrifugal and Coriolis forces, ˇ μ(q, q), ˙ at qo and q˙o for the constrained system. (i) Compute the task/constraint space vector of gravity forces, p(q), ˇ at qo for the constrained system.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:15:35, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.010

9

Applications to Biomechanical Systems

The biomechanics of motion provides a rich domain for the application of the principles of analytical dynamics. The skeletal systems of vertebrate animal species are well modeled as collections of interconnected rigid bodies (bones) with joints being parameterized by generalized coordinates. Additional complex coupling between the generalized coordinates/velocities can be accommodated with constraints – either holonomic constraints or nonholonomic rolling constraints. The musculoskeletal system comprises the skeletal system and a collection of musculotendon actuators that generate moments about the joints. In this chapter we will address dynamical models of the musculoskeletal and neuromuscular systems. The latter model describes how neural excitations result in contractile forces in the muscles, and the former model describes how musculotendon forces drive biomechanical motion.

9.1

Musculoskeletal and Neuromuscular Dynamics The musculoskeletal model presented here involves a description of the skeletal system as a rigid multibody system spanned by a set of musculotendon actuators. The musculotendon actuator model consists of a standard two-state Hill-type model (Schutte 1992; Zajac 1993). A stiff tendon simplification of the two-state model will also be presented.

9.1.1

Musculoskeletal Dynamics We begin by providing a description of the musculoskeletal system. A general branching chain multibody system is described by the standard system of n equations in configuration space τ = M (q)q¨ + b(q, q) ˙ + g(q).

(9.1)

Equation (9.1) can be used to model musculoskeletal dynamics, however, idealized torques at the joints, τ, do not represent the mechanism by which the musculoskeletal system is actuated. We now consider a set of r musculotendon actuators spanning the skeletal system. We will assume that the vector of musculotendon lengths, l ∈ Rr , can be uniquely determined from the system configuration, q. That is, l = l (q). As a consequence of this

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

236

Applications to Biomechanical Systems

assumption, differential variations in l are given by δl =

∂l δq = L(q)δq, ∂q

(9.2)

where L(q) ∈ Rr×n is the muscle Jacobian. From the principle of virtual work, we conclude that τ = −LT fT = R(q) fT ,

(9.3)

where fT ∈ Rr is the vector of tendon forces. The negative sign is due to the convention of taking contractile muscle forces as positive. The matrix of muscle moment arms is denoted R(q) ∈ Rn×r . From (9.1) and (9.3), the dynamics of the musculoskeletal system can be modeled as ˙ + g(q). R(q) fT = M (q)q¨ + b(q, q)

9.1.2

(9.4)

Neuromuscular Dynamics The behavior of the musculotendon actuators can be modeled as a set of Hill-type active state force-generating units (Zajac 1989, 1993). Activation dynamics refers to the process of muscle activation in response to neural excitation. This process can be modeled by the following equation of state written in terms of the vector of muscle activations, a ∈ Rr : ⎛ ⎞−1 0 τ (u1 , a1 ) 0 ⎜ ⎟ .. ⎟ (9.5) a˙ = ⎜ . 0 0 ⎠ (u − a), ⎝ 0

0

τ (ur , ar )

where ai ∈ [0, 1] and where ui ∈ [0, 1] is the neural input (excitation). The term τ (ui , ai ) is a time constant given by ⎧ ⎨(τa − τd )ui + τd , if ui ≥ ai (9.6) τ (ui , ai ) = ⎩τd , if ui < ai , where τa and τd are the activation and deactivation time constants, respectively. Contraction dynamics refers to the process of force generation in the muscle based on muscle contraction, rate of contraction, and activation. This process can be modeled as the lumped parameter system of Figure 9.1, which describes the configuration of forces. There is an active element, a passive viscoelastic element (in parallel), and an elastic tendon element (in series). The relative angle associated with the muscle fibers, α, is referred to as the pennation angle. From Figure 9.1 we have the following relations: l (q) = lM cos(α) + lT l˙(q) = l˙M cos(α) + l˙T ,

(9.7)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.1 Musculoskeletal and Neuromuscular Dynamics

237

Figure 9.1 Active state musculotendon model. The active contractile element and passive

viscoelastic element are in parallel. The passive elastic tendon element is in series.

where lM is the vector of muscle lengths and lT is the vector of tendon lengths. The following force equilibrium equation can be expressed: fT = ( fA + fP ) cos(α) = fM cos(α),

(9.8)

where fM and fT are the vectors of forces in the muscles and tendons, respectively, and fA and fP are the vectors of active and passive forces in the muscles, respectively. Using this force equilibrium equation as well as constitutive relationships describing muscle forces as a function of muscle length and contraction rate, fA (lM , l˙M , a) and fP (lM , l˙M ), and tendon force as a function of tendon length, fT (lT ), we can express the following equation of state in functional form: ˙ lM , a). l˙M = l˙M (l (q), l˙(q, q),

(9.9)

So, for a system of r musculotendon actuators, we can express the following system of 2r first-order state equations: ⎛ ⎜ a˙ = ⎜ ⎝

τ (u1 , a1 ) 0

0 .. .

0 0

0

0

τ (ur , ar )

˙ lM , a) l˙M = l˙M (l (q), l˙(q, q),

⎞−1 ⎟ ⎟ ⎠

(u − a)

(9.10)

(9.11)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

238

Applications to Biomechanical Systems

Figure 9.2 Neuromuscular and musculoskeletal system (feed-forward path). Neural excitations

provide input to the activation dynamics. Output of the activation dynamics provides input to the contraction dynamics. Output of the contraction dynamics provides input to the musculoskeletal dynamics through the tendon forces.

where the internal states are lM and a. The tendon force can be written in terms of the states as fT = fT (l (q), lM ).

(9.12)

The musculoskeletal equation (9.4) and neuromuscular equations (9.10), (9.11), and (9.12) can be represented in block diagram form as depicted in Figure 9.2.

9.1.3

Stiff Tendon Model If we make the assumption that the tendon is infinitely stiff, lM is no longer an independent state but rather is algebraically related to the overall musculotendon length, l(q). This provides a useful simplification to the full musculoskeletal dynamics described in the previous sections. We can define the muscle saturation force, fS , as the active muscle

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.1 Musculoskeletal and Neuromuscular Dynamics

239

force at full activation (a = 1). That is, fS (q, q) ˙  fA (q, q, ˙ 1). We can also define the muscle force-activation gain, K f , as the magnitude of force generation in the muscle per change in unit activation. Thus, ∂ fT = fS cos α. ∂a

Kf 

For a system of muscles, the muscle force-activation gain matrix is then ⎞ ⎛ fS1 cos α1 0 ⎟ ⎜ .. ⎟, Kf = ⎜ . ⎠ ⎝

(9.13)

(9.14)

fSr cos αr

0

and the muscle forces can now be expressed in terms of this gain: fT (q, q, ˙ a) = fP (q, q) ˙ + K f (q, q)a, ˙

(9.15)

T

fP = fP1 cos α1 · · · fPr cos αr .

(9.16)

where

As with the musculotendon forces, it is useful to express our generalized forces using a gain relationship. To this end, we will define the muscle torque-activation gain matrix: Kτ  RK f .

(9.17)

The generalized forces can now be expressed in terms of this gain: τ(q, q, ˙ a) = τP (q, q) ˙ + Kτ (q, q)a, ˙

(9.18)

τP = R fP .

(9.19)

where

The overall neuromusculoskeletal dynamics can then be expressed as ⎛ ⎞−1 τ (u1 , a1 ) 0 0 ⎜ ⎟ .. ⎟ a˙ = ⎜ . 0 0 ⎠ (u − a) ⎝ 0

0

(9.20)

τ (ur , ar )

˙ = M (q)q¨ + b(q, q) ˙ + g(q) − τP (q, q). ˙ Kτ (q, q)a

(9.21)

Example: Consider the n = 3 degree-of-freedom biomechanical model of Figure 9.3. The arm kinematics are shown in Table 9.1. The superscript t refers to the torso, h to the humerus, u to the ulna, r to the radius, and m to the hand frame of reference. The terms, thQ, tuQ, trQ, and tmQ are the rotation matrices of the humerus, ulna, radius, and hand, respectively, in the torso frame. The terms t rh , t ru , t rr , and t rm are the positions of the proximal ends of the humerus, ulna, radius, and hand, respectively, in the torso frame, and d1 , d2 , d3 , and d4 are fixed translation vectors.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

240

Applications to Biomechanical Systems

Table 9.1 Kinematics for a 3 degree-of-freedom human arm model Translation

Rotation Humerus

t

rh = d1

Q = Qz (q1 )

t

ru = rh + Qd2

t h

Ulna t

Q = thQQz (q2 )

t h

t u

Radius t

rr = t ru + tuQd3

Q = tuQ

t r

Hand t

rm = t rr + tuQd4

Q = tuQQz (q3 )

t m

The positions of the centers of mass of the bodies are given by t

rGh = t rh + thQhd−hG−→h ,

(9.22)

t

rGu = t ru + tuQud−uG−→u ,

(9.23)

rGr = t rr + trQrd−rG→r ,

(9.24)

t

t

−−→ . rGm = t rm + tmQmd−mG m

(9.25)

Figure 9.3 A simplified model of the human arm consisting of three generalized coordinates.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.1 Musculoskeletal and Neuromuscular Dynamics

241

The translational velocity Jacobians are given by t

Gh =

∂ t rGh , ∂q

(9.26)

t

Gu =

∂ t rGu , ∂q

(9.27)

Gr =

∂ t rGr , ∂q

(9.28)

Gm =

∂ t rGm . ∂q

(9.29)

t

t

For simplicity we will take the rotational inertias of the bodies to be zero. The dynamic terms are then M (q) = Mh t TGh t Gh + Mu t TGu t Gu + Mr t TGr t Gr + Mm t TGm t Gm , ⎛ T ∂M ⎞ q˙ ∂q1 q˙ ⎜ 1 ⎜ T ∂M ⎟ ⎟ b(q, q) ˙ = M˙ q˙ − ⎜ q˙ ∂q2 q˙ ⎟ , ⎠ ⎝ 2 q˙T ∂M q ˙ ∂q3 g(q) = (Mh t TGh + Mu t TGu + Mr t TGr + Mm t TGm )geˆ03 .

(9.30)

(9.31)

(9.32)

For the musculotendon units we will employ a stiff tendon model. We will also make two additional simplifications. We will take the pennation angle, α, and the passive force in the muscle, fP , to be zero. Thus, ⎞ ⎛ 0 fS1 ⎟ ⎜ .. ⎟ a. fT = fA = ⎜ (9.33) . ⎠ ⎝ 

0



fSr



Kf

The muscles for the arm are shown in Figure 9.4, and the muscle properties are summarized in Table 9.2. The muscle attachment and force-length data were taken from the study of Holzbaur, Murray, and Delp (2005). The saturation muscle force for a given muscle will be modeled as  ˙    l −l 2  lM −5 Ml Mo Mo erf 2 +1 , (9.34) f S = f = f oe vMo where the terms fo, lMo , and vMo are the maximum isometric force, optimal fiber length, and maximum muscle fiber contraction rate, respectively. It is noted that in the stiff tendon model, lM = l(q) − lTo

and

˙ q), l˙M = l(q, ˙

(9.35)

where lTo is a constant tendon slack length.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

242

Applications to Biomechanical Systems

Table 9.2 Maximum isometric forces, fo , optimal fiber lengths, lMo , tendon slack lengths, lTo , and maximum contraction rate, vMo , for the 24 muscles used in the model of Figure 9.4 Muscle

Joints Spanned

fo (N)

lMo (cm)

lTo (cm)

vMo (cm/s)

Input

DELT2 DELT3 SUPSP INFSP SUBSC TMAJ PECM1 PECM2 CORB TLONG TLAT TMED BLONG BSHORT BRA BRD ECRL ECRB PT EDCM EIP EPL FDSM FDPM

shoulder shoulder shoulder shoulder shoulder shoulder shoulder shoulder shoulder shoulder, elbow elbow elbow shoulder, elbow shoulder, elbow elbow elbow elbow, wrist elbow, wrist elbow, wrist elbow, wrist wrist wrist wrist wrist

1142.60 259.88 487.82 1210.84 1377.81 425.39 364.41 515.41 242.46 798.52 624.30 624.30 624.30 435.56 987.26 261.33 304.89 100.52 566.22 35.32 21.70 39.46 91.03 81.65

10.78 13.67 6.82 7.55 8.73 16.24 14.42 13.85 9.32 13.40 11.38 11.38 11.57 13.21 8.58 17.26 8.10 5.85 4.92 7.24 5.89 5.40 7.49 8.35

10.95 3.80 3.95 3.08 3.30 2.0 0.28 8.90 9.70 14.30 9.80 9.08 27.23 19.23 5.35 13.30 22.40 22.23 9.80 33.50 18.60 22.05 29.50 29.30

100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100

u1 u2 u3 u4 u5 u6 u7 u8 u9 u10 u11 u12 u13 u14 u15 u16 u17 u18 u19 u20 u21 u22 u23 u24

Figure 9.4 The simplified model of the human arm actuated by 24 muscles.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.1 Musculoskeletal and Neuromuscular Dynamics

243

Figure 9.5 Muscle force-length-rate surface at full activation. The terms fo, lMo , and vMo are the maximum isometric force, optimal fiber length, and maximum contraction rate, respectively. These are used as normalizing constants.

Normalizing (9.34) yields 

f −5 =e fo

lM lMo

2 −1

  ˙   lM erf 2 +1 . vMo

(9.36)

A plot of this normalized function is shown in Figure 9.5. Finally, the activation dynamics will be modeled as a˙ =

1 (u − a), τ

(9.37)

where an activation time constant, τ , of 20 ms is used. The overall neuromusculoskeletal system dynamics can then be expressed as a˙ =

1 (u − a) τ

Kτ (q, q)a ˙ = M (q)q¨ + b(q, q) ˙ + g(q).

(9.38) (9.39)

We can now simulate the dynamics of this system. For our neural input we will use a predetermined time history, u(t ), computed to produce a specific trajectory. Plots of the time histories of the hand trajectory, muscle excitations, and activations are shown in Figures 9.6 and 9.7.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

244

Applications to Biomechanical Systems

Figure 9.6 (Top) Animation frames from the simulation of a simplified model of the human arm

actuated by 24 muscles. (Bottom) Time history of Cartesian coordinates of the hand, x, following a specified trajectory.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

245

Figure 9.7 (Top) Time history of neural excitations, u (input). (Bottom) Time history of muscle

activations, a.

9.2

Constrained Dynamics of Biomechanical Systems The biomechanical study of human motion as well as the design of anthropomorphic robotic mechanisms require faithful representations of human skeletal kinematics. In recent years, there has been a proliferation of humanoid robotic systems. Human skeletal kinematics has been modeled in these systems at a basic level, but some important

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

246

Applications to Biomechanical Systems

aspects have been oversimplified or overlooked. While the representation of any skeletal joint as an ideal revolute or spherical joint is only an approximation, it is typically an acceptable one. Exceptions to this include the knee joint, which does not rotate about an absolute center but translates, as well, during knee extension (Delp et al. 1990). This added complexity, along with the presence of the patella (kneecap), has a significant influence on the generation of muscle moments about the knee. This is an important consideration if one wishes to simulate the human knee or emulate it in a humanoid robot that is to be driven by artificial muscles or cables. Thus, properly addressing the complexity of skeletal kinematics is important for both biomechanical simulations and anthropomorphic robot design.

9.2.1

Muscle-Based Actuation of Constrained Systems In biomechanical simulations it is desirable to actuate the constrained biomechanical system using a set of musculotendon actuators. By using either a stiff tendon model or ˙ a). a steady state evaluation of the musculotendon forces, we can express fT = fT (q, q, In either case the joint moments induced by these musculotendon forces are τ = R(q) fT .

(9.40)

The constrained equation of motion can thus be expressed in terms of muscle actuation: R fT (q, q, ˙ a) + T λ = M q¨ + b + g. T Premultiplying (9.41) by J¯ˇ gives the operational space form     0 x ¨ ˇ T (q) fT (q, q, ˙ a) + = + μˇ + p, ˇ λ 0

(9.41)

(9.42)

T where T (q) = J¯ˇ R ∈ R(mT +mC )×r . Our task motion equation can then be expressed as

ˇ 11 x T1 fT (q, q, ˙ a) =  ¨ + μˇ 1 + pˇ 1 ,

(9.43)

and the equation corresponding to constraint forces can be expressed as ˇ 21 x T2 fT (q, q, ˙ a) + λ =  ¨ + μˇ 2 + pˇ 2 ,

(9.44)

where T1 is the mT × r submatrix of T and T2 is the mC × r submatrix of T . Example: Perhaps the most kinematically complicated system in the human skeletal system is the shoulder complex. While the purpose of the shoulder complex is to produce spherical articulation of the humerus, the resultant motion does not exclusively involve motion of the glenohumeral joint (see Figure 9.8). The shoulder girdle, which comprises the clavicle and scapula, connects the glenohumeral joint to the torso and produces some of the motion associated with the overall articulation of the humerus. While this motion is small compared to the glenohumeral motion, its impact on overall arm function is significant (Klopˇcar and Lenarˇciˇc 2001;

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

247

Figure 9.8 Various constituents of the shoulder complex including the scapula, clavicle, and

humerus. The glenohumeral joint produces spherical motion of the humerus. The shoulder girdle attaches the glenohumeral joint to the torso and influences the resultant motion of the humerus through scapulothoracic, sternoclavicular, and acromioclavicular motion.

Lenarˇciˇc and Parenti-Castelli 2000). This impact is associated not only with the influence of the shoulder girdle on the skeletal kinematics of the shoulder complex but also with its influence on the routing and performance of muscles spanning the shoulder. As a consequence, shoulder kinematics is tightly coupled to the behavior of muscles spanning the shoulder. In turn, the action of these muscles (moments induced about the joints) influences the overall musculoskeletal dynamics of the shoulder. This coupling is illustrated in Figure 9.2. Muscle activation, a, causes force generation in the muscles. Force generation, fT , is dependant on muscle length, contraction rate, and musculotendon length (lM , l˙M , and l), which in turn are dependant on skeletal configuration (q and q). ˙ The muscle-induced joint moments, τ, are dependent on these muscle forces as well as on muscle moment arms, R (which again are dependent on skeletal kinematics). Finally, these joint moments influence the multibody dynamics of the skeletal system. We address the upper extremity model of Holzbaur et al. (2005) in this example. The model consists of a shoulder complex as well as a lower arm model. Holzbaur et al. implemented their model in the SIMM environment (Delp and Loan 2000) and, later, in OpenSim (Delp et al. 2007; Saul et al. 2015), where a minimal set of three generalized coordinates was chosen to describe the configuration of the shoulder complex. Since a minimal set of coordinates as employed in Holzbaur et al. (2005) the constraints that model the shoulder girdle are implicitly handled. Thus, all motions of the shoulder girdle are dependent on the three glenohumeral rotation coordinates. These are elevation plane, h1 , elevation angle, h2 , and shoulder rotation, h3 .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

248

Applications to Biomechanical Systems

Figure 9.9 Parameterization of a shoulder model using a nonminimal set coordinates, q6 , q7 , and q9 , corresponding to the glenohumeral rotations, h1 , h2 , and h3 , in the model of Holzbaur et al. (2005). Five holonomic constraints couple the movement of the shoulder girdle with the glenohumeral rotations.

The constrained movement of the shoulder girdle was determined from the shoulder rhythm regression analysis of de Groot and Brand (2001). The model obtained from this regression analysis was shown to fit well for an independent set of shoulder motions and on a different set of subjects than was used for the regression analysis (de Groot and Brand 2001). The model of de Groot and Brand is considered to be superior in predicting shoulder motion to a simple unconstrained model that only reflects glenohumeral rotation. For the purposes of formulating the dynamics it is often preferable to use a nonminimal, but standardized, set of generalized coordinates that are amenable to numerical formulation. Additionally, it is preferable to use a parameterization that preserves the physical meaning of the generalized forces as torques about individual joints. Often, when using a minimal set of coordinates, this is not the case, since a single generalized coordinate may influence multiple joint rotations, as in the parameterization of Holzbaur et al. (2005). For these reasons we will reparameterize the model of Holzbaur et al. (2005) to include a total of n = 9 generalized coordinates to describe the unconstrained configuration of the shoulder (De Sapio, Holzbaur, and Khatib 2006). As shown in Figure 9.9, the coordinates q6 , q7 , and q9 correspond to the independent coordinates for the shoulder complex used in Holzbaur et al. (2005): elevation plane, elevation angle, and shoulder rotation, respectively. The shoulder kinematics associated with this nonminimal set of generalized coordinates is shown in Table 9.3. The superscript t refers to the torso, c to the clavicle, s to the

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

249

Table 9.3 Shoulder kinematics using a nonminimal set of coordinates Translation

Rotation Clavicle

t

rc = d1

Q = Q1 (q1 )Q2 (q2 )

t

rs = rc + Qd2

t c

Scapula t

Q = Q3 (q3 )Q4 (q4 )Q5 (q5 )

t c

t s

Humerus t

rh = t rs + tsQd3

Q = Q6 (q6 )Q7 (q7 )Q6 (q8 )Q6 (q9 )

t h

scapula, and h to the humerus frame of reference. The terms tcQ, tsQ, and thQ are the rotation matrices of the clavicle, scapula, and humerus, respectively, in the torso frame, and Q1 , . . . , Q7 are rotation matrices associated with spins about successive local coordinate axes, where the arguments identify the spin angles. The terms t rc , t rs , and t rh are the positions of the proximal ends of the clavicle, scapula, and humerus, respectively, in the torso frame, and d1 , d2 , and d3 are fixed translation vectors. The constraint constants, b, associated with the dependency on humerus elevation plane, q6 , and c, associated with the dependency on humerus elevation angle, q7 , were obtained from the regression analysis of de Groot and Brand (2001). They are

b = 0.120 −0.046

c = −0.242 0.123

0.140 −0.049

−0.079 0.396

 −0.028  0.184 .

(9.45) (9.46)

Five holonomic constraints need to be imposed to properly constrain the motion of the shoulder girdle. With an additional constraint at the glenohumeral joint, we have a total of mC = 6 constraints. This yields p = n − mC = 3 degrees of kinematic freedom. Since this framework does not limit the dependent coordinates to functions of only a single independent coordinate, as in the case of the SIMM model (Holzbaur et al., 2005), we can implement the complete set of shoulder rhythm constraints (de Groot and Brand 2001) for our analysis. These constraint equations, φ(q) = 0, are given by ⎛

q1 − b1 q6 − c1 q7

⎞

⎟ ⎜ ⎜ q2 − b2 q6 − c2 q7 ⎟ ⎟ ⎜ ⎟ ⎜ ⎜ q3 − b3 q6 − c3 q7 ⎟ ⎟ = 0. ⎜ φ(q) = ⎜ ⎟ ⎜ q4 − b4 q6 − c4 q7 ⎟ ⎟ ⎜ ⎜q − b q − c q ⎟ 5 6 5 7⎠ ⎝ 5 q8 + q6

(9.47)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

250

Applications to Biomechanical Systems

Taking the derivative yields ⎛

q˙1 − b1 q˙6 − c1 q˙7

⎞

⎟ ⎜ ⎜ q˙2 − b2 q˙6 − c2 q˙7 ⎟ ⎟ ⎜ ⎟ ⎜ ⎜ q˙3 − b3 q˙6 − c3 q˙7 ⎟ ⎟ = q˙ = 0. φ˙ = ⎜ ⎟ ⎜ ⎜ q˙4 − b4 q˙6 − c4 q˙7 ⎟ ⎟ ⎜ ⎜ q˙ − b q˙ − c q˙ ⎟ 5 6 5 7⎠ ⎝ 5 q˙8 + q˙6

(9.48)

So, ⎛

1

⎜ ⎜0 ⎜ ⎜ ⎜0 ⎜ q˙ = ⎜ ⎜0 ⎜ ⎜ ⎜0 ⎝ 0

0

0

0

0

−b1

−c1

1

0

0

0

−b2

−c2

0

1

0

0

−b3

−c3

0

0

1

0

−b4

−c4

0

0

0

1

−b5

−c5

0

0

0

0

1

0

0 0

⎞

⎟⎛ ⎞ q˙ 0 0⎟ ⎟⎜ 1⎟ ⎟⎜ . ⎟ .. ⎟ 0 0⎟ ⎟⎜ ⎜ ⎟ = 0, ⎟ .. ⎟ 0 0⎟⎜ ⎟ ⎟⎜ ⎟⎝ . ⎠ 0 0⎟ ⎠ q˙9 1 0

(9.49)

where ⎛

1

⎜ ⎜0 ⎜ ⎜ ⎜0 =⎜ ⎜ ⎜0 ⎜ ⎜0 ⎝ 0

0

0

0

0 −b1

−c1

1

0

0

0 −b2

−c2

0

1

0

0 −b3

−c3

0

0

1

0 −b4

−c4

0

0

0

1 −b5

−c5

0

0

0

0

1

0

0 0

⎞

⎟ 0 0⎟ ⎟ ⎟ 0 0⎟ ⎟. ⎟ 0 0⎟ ⎟ 0 0⎟ ⎠ 1 0

(9.50)

The positions of the centers of mass of the bodies are given by t

rGc = t rc + tcQcd−cG−→c ,

(9.51)

t

−→ , rGs = t rs + tsQsdsG s

(9.52)

rGh = t rh + thQhd−hG−→h .

(9.53)

t

We can compute the angular velocities in the local frame by noting that ˙ c = tcQT tcQ,

(9.54)

˙ s = tsQT tsQ,

(9.55)

˙ h = thQT thQ.

(9.56)

c

s

h

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

251

The translational velocity Jacobians are given by t

Gc =

∂ t rGc , ∂q

(9.57)

t

Gs =

∂ t rGs , ∂q

(9.58)

Gh =

∂ t rGh , ∂q

(9.59)

t

and the angular velocity Jacobians are given by c =

∂ cωc , ∂ q˙

(9.60)

s

s =

∂ sωs , ∂ q˙

(9.61)

h =

∂ hωh . ∂ q˙

(9.62)

c

h

The dynamic terms are then M (q) = Mc t TGc t Gc + Ms t TGs t Gs + Mh t TGh t Gh + cTc cIcGc cc + sTs sIsGs ss + hTh hIh h hh , ⎛ T ∂M ⎞ q˙ ∂q1 q˙ ⎜ 1⎜ . ⎟ ⎟ b(q, q) ˙ = M˙ q˙ − ⎜ .. ⎟ , ⎠ 2⎝ q ˙ q˙T ∂M ∂q9 G

g(q) = (Mc t TGc + Ms t TGs + Mh t TGh )geˆ03 .

(9.63)

(9.64)

(9.65)

The constrained equation of motion is τ + T λ = M q¨ + b + g,

(9.66)

or, expressed in terms of muscle actuation, R fT (q, q, ˙ a) + T λ = M q¨ + b + g.

(9.67)

Using the configuration space properties computed, we can formulate the task space dynamics. We will specify the task to be the orientation angles of the humerus (see Figure 9.9): T

(9.68) x = q6 q7 q9 . The task Jacobian is

⎛ 0 ⎜ ∂x ⎜ J (q) = = ⎝0 ∂q 0

0

0

0

0

1

0

0

0

0

0

0

0

1

0

0

0

0

0

0

0

0

⎞ 0 ⎟ 0⎟ ⎠. 1

(9.69)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

252

Applications to Biomechanical Systems

Figure 9.10 Muscle paths spanning the shoulder complex. A shoulder abduction sequence is

shown. The movement of the scapula, a constituent of the shoulder girdle, can be observed. This movement influences the paths of the muscle-tendon units wrapping over the scapula. Since the muscle moment arms are determined from the muscle-tendon path data, the motion of the shoulder girdle influences the moment arms about the glenohumeral joint.

Using J (q), as well as (q), M (q), b(q, q), ˙ and g(q), we can compute the task/constraint space terms,  −1 JM −1 J T JM −1 T ˇ , (9.70) (q) = M −1 J T M −1 T   JM −1 b − J˙q˙ ˇ ˇ μ(q, q) ˙ = , (9.71) ˙ q˙ M −1 b −    JM −1 g ˇ p(q) ˇ = . (9.72) M −1 g As in the previous examples, since these terms involve matrix inverses, it is preferable to compute them numerically at each time step within the numerical integration loop rather than computing them symbolically. Our task/constraint space equation of motion for the dynamics at the task point is ˇ 11 x ¨ + μˇ 1 + pˇ 1 , f =

(9.73)

and the equation corresponding to constraint forces in the system is ˇ 21 x fC + λ =  ¨ + μˇ 2 + pˇ 2 .

(9.74)

The muscle-actuated task motion equation is ˇ 11 x T1 fT (q, q, ˙ a) =  ¨ + μˇ 1 + pˇ 1 ,

(9.75)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

253

and the equation corresponding to constraint forces can be expressed as ˇ 21 x T2 fT (q, q, ˙ a) + λ =  ¨ + μˇ 2 + pˇ 2 .

(9.76)

The dynamics of the constrained shoulder model addressed here, which involves kinematic coupling between the humerus, scapula, and clavicle, differs from a simple unconstrained shoulder model. The constrained model also differs from a simple model in the degree to which the system of muscles is able to generate control forces for a given motion task. This is due to the influence of the constrained motion between the humerus, scapula, and clavicle on the muscle forces and muscle moment arms about the glenohumeral joint (see Figure 9.10). An example of this is shown in Figures 9.11 and 9.12. Predicted muscle moment arms, muscle forces, and moment-generating capacities for the deltoid muscles are compared for the simple and constrained shoulder models. The muscle path and force-length data were taken from the study of Holzbaur et al. (2005). In the constrained shoulder model the motions of the scapula and clavicle are highly coupled to humerus elevation angle (q7 coordinate), whereas in the simple shoulder model the motions of the scapula and clavicle are not coupled to glenohumeral motion. The paths of the deltoid muscles are affected by the constrained motion of the humerus, scapula, and clavicle. This results in significant differences in moment arms predicted by the two models, with the constrained model often generating moment arms of substantially larger magnitude than the simple model. Additionally, the predicted isometric muscle forces (computed at full activation) generated by the two models differ. The resulting moment-generating capacities of the constrained model are often substantially larger in magnitude than the simple model. This implies that the simple model, which excludes the constrained shoulder girdle motion, typically underestimates the moment-generating capacities of muscles that span the shoulder, since Holzbaur et al. (2005) demonstrated correlation between predicted and experimental moment-generating capacities for the constrained model. This is critical in various application areas involving the study and synthesis of human movement (De Sapio, Khatib, and Delp 2005).

Example: The purpose of the biomechanical human shoulder model discussed in the previous section is to simulate physiological shoulder motion and musculotendon routing. As such, it is ultimately intended to be actuated in a physiological manner, that is, by a system of musculotendon actuators that simulate skeletal muscle. For robotic applications, a mechanical analog of the human shoulder may be sought. If this mechanical analog is to be actuated by artificial muscles or cables, it is desirable to emulate human shoulder kinematics and muscle routing to reproduce the human-like action of muscles (moment arms) about the joints. Standard robotic actuation may be adapted to a humanoid robotic shoulder complex rather than actuation that attempts to emulate musculoskeletal physiology. This offers the advantages of human shoulder kinematics without requiring complicated actuation. An example of such a system is the shoulder complex proposed by Lenarˇciˇc and

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

254

Applications to Biomechanical Systems

Figure 9.11 (Top) Muscle paths for the deltoid muscles. Elevation angle, q7 is varied. (Bottom) Muscle moment arms for the deltoid muscles, as predicted by the constrained and simple shoulder models. The constrained model typically generates moment arms of substantially larger magnitude than those of the simple model.

colleagues (Lenarˇciˇc and Parenti-Castelli 2000; Lenarˇciˇc and Staniši´c 2003). Their mechanism consists of a parallel-serial kinematic structure with four actuated prismatic joints (parallel part) and three actuated revolute joints (serial part). The parallel part, consisting of four extensible legs attached to a moveable platform, acts as the shoulder girdle that supports the glenohumeral joint. As such, its

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

255

Figure 9.12 (Top) Muscle forces for the deltoid muscles, as predicted by the constrained and

simple shoulder models. (Bottom) Moment-generating capacities for the deltoid muscles, as predicted by the constrained and simple shoulder models. The moment-generating capacities associated with the constrained model are typically larger in magnitude than those associated with the simple model.

design is intended to emulate the functionality of the scapula and clavicle attached to a fixed torso and connected by the scapulothoracic, sternoclavicular, and acromioclavicular joints. A parallel kinematic structure was chosen because of the need for high stiffness and precision in orienting the attached glenohumeral joint (Lenarˇciˇc and Parenti-Castelli 2000). The serial part consists of a spherical glenohumeral joint

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

256

Applications to Biomechanical Systems

Figure 9.13 The parallel and serial parts composing the humanoid shoulder complex proposed by

Lenarˇciˇc et al. For each shoulder a total of 16 generalized coordinates are employed (n = 16). These are constrained by 9 holonomic constraints (mC = 9), yielding 7 degrees of freedom (p = 7).

attaching the humerus link to the shoulder girdle. A bilateral version of this is shown in Figure 9.13. Lenarˇciˇc et al. presented detailed kinematic analyses of their shoulder complex design. For our purposes we will present a kinematic parameterization and constraint definition suitable for use in our constraint-based control framework. The system is partitioned into four serial chains with a total of 16 generalized coordinates defined as shown in Figure 9.14. Holonomic loop constraints need to be imposed that reflect the connection of the extensible legs to the moveable platform. These constraints are expressed as rli = r pi

for i = 1, . . . , 3,

(9.77)

Figure 9.14 (Left) Humanoid shoulder complex actuated by four prismatic struts (remaining

joints are passive). (Right) The closed-loop mechanism is cut at various locations to create four serial subsystems described by the set of generalized coordinates, q1 , . . . , q24 .

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

257

where rli is the terminal point of the ith strut subsystem that connects to r pi , the ith position on the platform subsystem. In vector form we have ⎞ ⎛ rl1 − r p1 ⎟ ⎜ ⎟ (9.78) φ(q) = ⎜ ⎝ rl2 − r p2 ⎠ = 0. rl3 − r p3 With these constraints the system possesses p = n − mC = 16 − 9 = 7 degrees of freedom (4 for the parallel part and 3 for the serial part). Taking the derivative yields ⎞ ⎛ r˙l1 − r˙p1 ⎟ ⎜ ⎟ (9.79) φ˙ = ⎜ ⎝ r˙l2 − r˙p2 ⎠ = q˙ = 0, r˙l3 − r˙p3 where ⎛

q˙3i−2

⎞

⎛

⎜ ⎟ r˙li = li ⎝ q˙3i−1 ⎠

and

⎞ q˙10 ⎜ ⎟ r˙pi =  pi ⎝ ... ⎠ , q˙13

q˙3i

(9.80)

for i = 1, . . . , 3. The terms li and  pi are the corresponding Jacobians of rli and r pi , respectively. So, ⎛

 l1 ⎜ q˙ = ⎜ ⎝0 0

⎞ ⎛ q˙1 ⎞ . ⎟ 0 ⎜ ⎜ . ⎟ ⎟⎜ . ⎟ ⎟ ⎜ 0⎟ ⎠ ⎜ q˙13 ⎟ = 0, ⎜ . ⎟ 0 ⎝ .. ⎠ q˙16

0

0

− p1

 l2

0

− p2

0

 l3

− p3

0

0

− p1

 l2

0

− p2

⎟ 0⎟ ⎠.

0

 l3

− p3

0

(9.81)

where ⎛  l1 ⎜ =⎜ ⎝0 0

0

⎞ (9.82)

The constraint forces, λ, are shown in Figure 9.15. The unconstrained equations of motion for the three strut serial subsystems are as follows: ⎛ ⎞ ⎛ ⎞ q¨3i−2 0 ⎟ ⎝ 0 ⎠ = Mli ⎜ ⎝ q¨3i−1 ⎠ + bli (q3i−2 , . . . , q˙3i ) + gli (q3i−2 , q3i−1 , q¨3i ), (9.83) τi q¨3i for i = 1, . . . , 3,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

258

Applications to Biomechanical Systems

Figure 9.15 Constraint forces associated with loop closures in the humanoid shoulder complex.

where Mli = Mli TGl Gli + Tli IlGi i li , i ⎛ ∂Ml ⎞ q˙T 1i q˙ ⎜ ∂q ⎟ 1 .. ⎟, bli = M˙ li q˙ − ⎜ . ⎝ ⎠ 2 ∂M q˙T ∂qnli q˙ gli = Mli TGl eˆ03 ,

(9.84)

(9.85)

(9.86)

i

for i = 1, . . . , 3. The terms Gli and Mli are the center of mass Jacobian and mass, respectively, of the ith strut. For the platform serial subsystem (including the 3 degree-of-freedom end link), we have ⎛ ⎞ τ4 ⎛ ⎞ ⎜0⎟ q¨10 ⎜ ⎟ ⎜ . ⎟ ⎜ ⎟ (9.87) ⎜ τ5 ⎟ = M p ⎝ .. ⎠ + b p (q10 , . . . , q˙16 ) + g p (q10 , . . . , q16 ), ⎜ ⎟ ⎝ τ6 ⎠ q¨16 τ7 where Mp =

3    M pi TG pi G pi + Tpi IpGi i  pi , i=1

⎛

1⎜ b p = M˙ p q˙ − ⎜ 2⎝

gp =

3 

q˙T

∂M p q˙ ∂q1

.. .

∂M q˙T ∂qnp q˙

M pi TG pi eˆ03 ,

(9.88)

⎞ ⎟ ⎟, ⎠

(9.89)

(9.90)

i=1

for i = 1, . . . , 3.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

9.2 Constrained Dynamics of Biomechanical Systems

259

The terms G pi and M pi are the center of mass Jacobian and mass, respectively, of the ith link in the platform chain serial subsystem. The entire unconstrained system is described by ⎛ ⎞ 0 ⎜ ⎟ ⎜ τ1 ⎟ ⎜ ⎟ ⎜0⎟ ⎜ ⎟ ⎜ ⎟ ⎛ ⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎜ τ2 ⎟ Ml1 0 0 0 q¨1 bl1 gl1 ⎜ ⎟ ⎜0⎟ ⎜ ⎟⎜ ⎟ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ 0 Ml2 ⎟ ⎜ q¨2 ⎟ ⎜ 0 0 ⎜g ⎟ ⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ bl2 ⎟ ⎟ + ⎜ l2 ⎟ , (9.91) ⎜ τ3 ⎟ = ⎜ ⎟⎜ . ⎟ + ⎜ ⎜ ⎜ ⎟ ⎟ ⎜ ⎜ ⎟ ⎜ 0 ⎟ ⎟ 0 0 Ml3 . ⎜ τ4 ⎟ ⎝ ⎠ ⎝ . ⎠ ⎝ bl3 ⎠ ⎝ gl3 ⎠ ⎜ ⎟ 0 0 0 Mp bp gp ⎜ ⎟ q¨16 ⎜0⎟ ⎜ ⎟ ⎜ τ5 ⎟ ⎜ ⎟ ⎜ ⎟ ⎝ τ6 ⎠ τ7 and the constrained system is ⎛ ⎞ 0 ⎜ ⎟ ⎜ τ1 ⎟ ⎜ ⎟ ⎜0⎟ ⎜ ⎟ ⎜ ⎟ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎞⎛ ⎜ τ2 ⎟ q¨1 Ml1 b g 0 0 0 ⎜ ⎟ ⎜0⎟ ⎜ ⎟ ⎜ l1 ⎟ ⎜ l1 ⎟ ⎟⎜ ⎜ ⎟ ⎜ 0 M ⎜ ⎟ q ¨ 2 ⎜ ⎜ ⎟ ⎟ 0 0 bl2 g ⎟ l2 ⎜ ⎟ ⎜ ⎟ ⎟⎜ ⎟ + ⎜ l2 ⎟ ⎜ τ3 ⎟ = ⎜ ⎜ . ⎟+⎜ ⎜ ⎜ ⎟ ⎟ ⎟ ⎜ ⎟ ⎝ 0 . ⎟ 0 ⎠⎜ 0 Ml3 ⎜ τ4 ⎟ ⎝ . ⎠ ⎝ bl3 ⎠ ⎝ gl3 ⎠ ⎜ ⎟ bp gp 0 0 0 Mp ⎜ ⎟ q¨16 ⎜0⎟ ⎜ ⎟ ⎜ τ5 ⎟ ⎜ ⎟ ⎜ ⎟ ⎝ τ6 ⎠ τ7 ⎛ ⎞ Tl1 0 0 ⎜ ⎟⎛ ⎞ ⎜ 0 0 ⎟ Tl2 ⎜ ⎟ λ1 ⎜ T ⎟ ⎜ .. ⎟ +⎜ 0 0  l3 ⎟ ⎝ . ⎠ , ⎜ ⎟ ⎜−T −T −T ⎟ λ 9 p2 p3 ⎠ ⎝ p1 0

0

(9.92)

0

where the dimensions of the terms are M ∈ Rn×n ,

b, g, τ ∈ Rn ,

 ∈ Rm×n ,

λ ∈ Rm ,

(9.93)

and n = 16 and m = 9. Using the configuration space properties computed, we can formulate the task space dynamics. We will specify the task to be the endpoint, rE , of the serial chain attached to

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

260

Applications to Biomechanical Systems

Figure 9.16 Humanoid shoulder complex showing task point.

the moving platform (see Figure 9.16): x = 0rE .

(9.94)

The task Jacobian is J (q) =

∂x . ∂q

(9.95)

Using J (q), as well as (q), M (q), b(q, q), ˙ and g(q), we can compute the task/constraint space terms:  ˇ (q) =

JM −1 J T

JM −1 T

M −1 J T M −1 T   JM −1 b − J˙q˙ ˇ ˇ μ(q, q) ˙ = , ˙ q˙ M −1 b −    JM −1 g ˇ p(q) ˇ = . M −1 g

−1 ,

(9.96)

(9.97)

(9.98)

As in the previous examples, since these terms involve matrix inverses, it is preferable to compute them numerically at each time step within the numerical integration loop rather than computing them symbolically. Our task/constraint space equation of motion for the dynamics at the task point is ˇ 11 x ¨ + μˇ 1 + pˇ 1 , f =

(9.99)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

261

9.2 Constrained Dynamics of Biomechanical Systems

Figure 9.17 (Top) Animation frames from the simulation of the humanoid shoulder complex

parallel shoulder mechanism. (Bottom) Time history of the humanoid shoulder complex task space gravity vector shoulder mechanism task space gravity vector.

and the equation corresponding to constraint forces in the system is ˇ 21 x fC + λ =  ¨ + μˇ 2 + pˇ 2 .

(9.100)

We will specify a task space force at the task point to cancel out the task/constraint space centrifugal and Coriolis force, and the task/constraint space gravity force. That is, f = μˇ 1 + pˇ 1 .

(9.101)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

262

Applications to Biomechanical Systems

This will result in the following dynamics: ˇ 11 x  ¨ = 0.

(9.102)

Therefore, the task point will have zero acceleration. Given zero velocity initial conditions, the task point will not move. The system will, however, move in the null space. Results from simulation of the system under gravity are shown in Figures 9.17.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:26:24, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.011

10

Software for Analytical Dynamics

This chapter provides a brief and very selective survey of some software useful for solving problems in analytical dynamics. I have chosen to include only software intended for academic and industrial use that has been validated for modeling accuracy. Consequently, I have omitted the many dynamics engines used for video games and entertainment purposes.

10.1

General Purpose Mathematical Software A number of powerful tools for mathematical analysis exist. Two of the most popular are Mathematica by Wolfram Research Inc. and MATLAB by The MathWorks Inc. They provide capabilities for both symbolic manipulation of expressions (i.e., computer algebra systems) and numerical computation and evaluation of expressions. While the basic Mathematica and MATLAB applications do not provide high-level programming abstractions specific to multibody system modeling and analysis, they are extremely useful for the low-level symbolic and numerical processing required in solving problems in analytical dynamics (see Figure 10.1 for a screenshot of Mathematica). This has a benefit in the learning process, as they require the student to understand the central concepts of analytical dynamics and to set up the solution procedure in a detailed manner, while alleviating much of the tedium of the algebraic and numerical work.

10.1.1

Packages and Extensions for Multibody Dynamics Wolfram Research and MathWorks both provide domain-specific extensions for analyzing multibody systems. This gives the user access to high-level abstractions for modeling, simulating, and analyzing multibody systems. These high-level abstractions can be accessed through a programming language or through a diagrammatic interface supporting both acausal and causal modeling. Acausal modeling involves component-based schematic representations of the physical system, whereas causal modeling involves block diagram representations of the underlying mathematical description of the system. Simscape Multibody (formerly SimMechanics) by The MathWorks Inc. allows users to model multibody systems using a library of blocks associated with bodies, joints, constraints, spatial transformations, force elements, and sensors (see Figure 10.2 for

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

264

Software for Analytical Dynamics

Figure 10.1 Mathematica by Wolfram Research Inc. offers general symbolic and numerical

capabilities suitable for modeling, analysis, and simulation of multibody systems. A gimballed gyroscope is modeled here. Reprinted with permission of Wolfram Research Inc. Mathematica® is a registered trademark of Wolfram Research Inc.

a screenshot of Simscape Multibody). Entire CAD assemblies can be imported along with associated geometry, inertial properties, joint parameters, and constraints. Simscape Multibody then formulates and integrates the equations of motion for the multibody system. Animations are generated during simulation to allow visualization of the motion of the system. Simscape Multibody also allows design and simulation of control systems in Simulink, which uses a causal block diagram interface for control system modeling. Simscape Multibody supports automatic C code generation for model deployment in other simulation environments, including hardware-in-the-loop (HIL) systems. In addition to the block diagrammatic user interface, the object-oriented Simscape

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

10.1 General Purpose Mathematical Software

265

Figure 10.2 Simscape Multibody by The MathWorks Inc. provides a multibody simulation

environment for 3D mechanical systems using blocks representing bodies, joints, constraints, force elements, and sensors. Reprinted with permission of The MathWorks Inc. Simscape™ Multibody™ is a trademark of The MathWorks Inc.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

266

Software for Analytical Dynamics

Figure 10.3 SystemModeler by Wolfram Research Inc. offers drag-and-drop component-based

modeling, analysis, and simulation of multibody systems. Reprinted with permission of Wolfram Research Inc. SystemModeler® is a registered trademark of Wolfram Research Inc.

language builds on the MATLAB programming language with features specific to physical modeling. SystemModeler by Wolfram Research provides multidomain modeling, simulation, and analysis capabilities (see Figure 10.3 for a screenshot of SystemModeler).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

10.1 General Purpose Mathematical Software

267

Figure 10.4 OpenSim biomechanical simulation software incorporating Simbody. Simbody’s

solver was derived from the NIH Internal Variable Dynamics Module (IVM) for molecular dynamics, itself based on the spatial operator algebra.

Like Simscape Multibody, SystemModeler allows users to create hierarchical component-based models that are consistent with the physical topology of the system. Models are translated into optimized equations of motion. Built-in numerical solvers handle systems with events and discontinuous behavior. As with Simscape, animations are generated during simulation to allow visualization of the motion of the system. SystemModeler incorporates Modelica, a modeling language. Modelica is a nonproprietary, object-oriented, equation-based language to conveniently model complex physical systems . Neweul-M2 , developed at the University of Stuttgart, is a software package for the analysis of multibody systems. It uses the MATLAB Symbolic Math Toolbox to generate the equations of motion for multibody systems in symbolic form. The formulation of the equations of motion is based on the Newton-Euler formalism and d’Alembert’s and Jourdain’s Principles. Currently constraints are restricted to holonomic ones. A command line interface and a graphical user interface are provided. Since the equations of motion are computed in symbolic form, they can be exported into the C programming language or used in a Simulink S-function.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

268

Software for Analytical Dynamics

10.2

Dedicated Multibody Dynamics Software

10.2.1

Commerical Software Adams (Automated Dynamic Analysis of Mechanical Systems) by MSC Software Corporation is a multibody dynamics simulation suite consisting of Adams, Adams Machinery, Adams Car, Adams Flexible Multibody Systems, and Adams MaxFlex. CAD assemblies can be imported from many major CAD systems. Multibody systems are created using libraries of joints and constraints. Simpack by SIMULIA (a division of Dassault Systèmes) is another dedicated multibody simulation environment used primarily in the automotive, railway, engine, wind turbine, power transmission, and aerospace industries. Simpack is specialized to address high-degree-of-freedom models with flexible bodies and impact between bodies.

10.2.2

Open Source Software Simbody was developed as part of SimBios, the NIH Center for Biomedical Computation at Stanford University (see Figure 10.4 for a screenshot of OpenSim, which uses the Simbody engine). It has been used for coarse-grained molecular models as well as large-scale mechanical models, such as neuromusculoskeletal models of human gait, and robotic systems. Simbody is provided as an open source, object-oriented C++ application programming interface (API). It was derived from the public domain NIH Internal Variable Dynamics Module (IVM) for molecular dynamics, itself based on the spatial operator algebra (Rodriguez, Jain, and Kreutz-Delgado 1991) developed at NASA’s Jet Propulsion Laboratory (JPL).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:11:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.012

Appendix Inclusion of Flexible Bodies

The previous chapters have detailed the dynamics of constrained multibody systems where the bodies are rigid. These methods can be extended to handle constrained systems involving a mix of flexible and rigid bodies. The method of absolute nodal coordinates described by Shabana (1998) has a demonstrated efficacy in application to flexible/rigid multibody systems. Using this method, the flexible subsystems are described using finite element nodal coordinates with respect to an absolute global coordinate system. For the rigid-body subsystems we can choose generalized coordinates that describe relative joint motion. Coupling the flexible and rigid subsystems results in a constrained dynamical system with graph topology. For conciseness, in the following sections, the Einstein summation convention will be used. This specifies that summation is implied over all values of the index when an index variable appears twice in a single term. Consequently, the summation symbol can be omitted in these cases. For example, Ai j eˆi ⊗ eˆ j =

3  3 

Ai j eˆi ⊗ eˆ j .

(A.1)

i=1 j=1

A.1

Continuum Kinematics Continuum kinematics refers to the study of the motion of a continuum without regard to the causes of that motion. Unlike the kinematics of discrete systems of points and rigid bodies, the kinematics of continuous systems involve infinitely divisible deformable material elements.

A.1.1

Deformation A deformation is formally defined as a diffeomorphism between a reference manifold, Mo, and a deformed manifold, Md . That is, {x|Mo → Md }: x = x(X , t ),

(A.2)

where x represents the spatial field coordinates and X represents the material coordinates. We can represent the inversion of this as X = X (x, t ).

(A.3)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

270

Inclusion of Flexible Bodies

The displacement, u(X , t ), of a material point is the difference between the deformed position vector, x(X , t ), and the reference position vector, X . The material description of displacement is u(X , t ) = x(X , t ) − X .

(A.4)

Using (A.3), we can express the spatial description as u(x, t ) = x − X (x, t ).

(A.5)

The material description of the material velocity is given by v(X , t ) =

∂u(X , t ) , ∂t

(A.6)

and the material description of the material acceleration is given by a(X , t ) =

∂ 2 u(X , t ) ∂v(X , t ) = . ∂t ∂t 2

(A.7)

Using (A.3), the spatial descriptions of the material velocity and material acceleration are v = v(x, t )

(A.8)

a = a(x, t ).

(A.9)

and

We can also arrive at (A.9) by taking the material derivative of the spatial description of velocity, a(x, t ) =

∂v(x, t ) Dv(x, t ) = + v∇[v(x, t )], Dt ∂t

(A.10)

∂v(x, t ) ∂t

(A.11)

where a(x, ˆ t) =

is the spatial description of the spatial acceleration vector.

A.2

Continuum Dynamics Continuum dynamics builds upon continuum kinematics by adding descriptions of the physical causes of motion. This includes both physical laws relating force or stress to acceleration (Newton’s law) and constitutive equations relating stress to material strain or strain rate. Given a finite element discretization, we can assemble the equations of motion for a flexible-body system. The absolute nodal coordinate method (Shabana 1998) is useful for dealing with systems involving large rigid body motions and/or large deformations. Kübler, Eberhard,

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

271

A.2 Continuum Dynamics

Figure A.1 Eight-node hexahedral element and coordinates of the intrinsic element parameter space, ξ , η, and ζ .

and Geisler (2003a, 2003b) provide an excellent description of the absolute nodal coordinate method with regard to flexible/rigid multibody systems. A brief review is presented here. For a given flexible body subsystem a Lagrangian, or material, description is chosen that relates all quantities to the reference configuration, X , of the system with domain o. Using a nonlinear finite element approach, a set of shape or interpolating functions, {N1 , . . . , Ns }, associated with a particular finite element discretization of s nodes can be chosen. In the case of eight-node isoparametric hexahedral elements (see Figure A.1), the interpolating functions are given as (Hughes 2000) Na (ξ) =

1 (1 + ξa ξ )(1 + ηa η)(1 + ζa ζ ), 8 for a = 1, 2, . . . , 8,

(A.12)

where ξ = ( ξ η ζ )T are the coordinates of the intrinsic element parameter space. Defining the shape matrix, N ∈ R3×3s , ⎛

N1 N ⎝0 0

0 N1 0

0 0 N1

··· ··· ···

Ns 0 0

0 Ns 0

⎞ 0 0 ⎠, Ns

(A.13)

the displacement field, u(X , t ) ∈ R3 , is given by u = Nd, where d ∈ R3s is the vector of nodal displacements. The current material configuration is then given by x(X , t ) = X + u(X , t ). The weak form statement (summation convention applied) of the flexiblebody dynamics problem is δd j (Mjk d¨k + k j + g j ) = 0,

(A.14)

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

272

Inclusion of Flexible Bodies

where

Mjk =

ρoNi j Nik dV ,

(A.15)

o

 

∂Ni j ∂Ni j ∂Nil δil + Fil Slk dV = dk Slk dV , ∂Xk ∂Xk ∂Xk o o

g j = − ρoNi j bi dV − Ni j pi dA.

kj =

o

(A.16)

(A.17)

∂o

The term ρo is the material density field in the reference configuration, Fil is the deformation gradient , bi are the body forces (e.g., gravity), pi are the surface tractions, δil is the Kronecker delta, and Slk is the second Piola-Kirchoff stress tensor. The system can thus be stated as M d¨ + k + g = 0,

(A.18)

M d¨ + k + g = f ,

(A.19)

or more generally as

where f ∈ R3s is a vector of external forces applied at the nodes. The terms M ∈ R3s×3s , k ∈ R3s , and g ∈ R3s are the mass matrix, stiffness vector, and body/surface force vector, respectively. It is noted that due to integration with respect to the reference configuration, o, the mass matrix, M, is constant. The stress tensor, S, is highly nonlinear, however. Different constitutive models (Belytschko et al., 2013) can be incorporated into (A.16) to provide a material specific means for computing S. Generally speaking, a constitutive model can express S = S(E, . . .), where E is the Green-Lagrange strain tensor and the ellipses represent functional dependence on rate terms (as in the case of viscous behavior). This allows us to relate the stress tensor to the displacements, since Elk =

1 (Fil Fik − δlk ) 2

(A.20)

and Fi j = δi j +

∂Ni j dk . ∂Xk

(A.21)

The constitutive relationships thus provide a means of evaluating k based on the system ˙ We will not consider any detailed constitutive models here but will rather states, d and d. focus on the mathematical structure of (A.19).

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

A.3 Subsystem Assembly

A.3

273

Subsystem Assembly We can assemble a set of rigid and flexible subsystems into a single constrained system. First, we specify the dynamics of a set of y unconstrained rigid-body subsystems, τ1 = Mr1 q¨1 + b1 + gr1 , .. .

(A.22)

τy = Mry q¨y + by + gry . Next, we specify the dynamics of a set of z flexible subsystems that have been discretized using absolute nodal coordinates, f1 = M f1 d¨1 + k1 + g f1 , .. . fz = M fz d¨z + kz + g fz .

(A.23)

The sets of equations given by (A.22) and (A.23) can be assembled into a single system equation of the form τ = M q¨ + b + g,

(A.24)

M = diag(Mr1 , . . . , Mry , M f1 , . . . , M fz ), T  T T T T , τ = τ 1 · · · τy f 1 · · · f z

(A.25)

where

T q¨ = q¨T1 · · · q¨Ty d¨1T · · · d¨zT , T

b = bT1 · · · bTy k1T · · · kzT ,  T g = gTr1 · · · gTry gTf1 · · · gTfz .

(A.26) (A.27) (A.28) (A.29)

We have M ∈ Rn×n and τ, q, ¨ b, g ∈ Rn , where n=

y  i=1

nri +

z 

nfi

(A.30)

i=1

nfi = 3si .

(A.31)

Imposing a set of mC holonomic constraint equations to establish system connectivity yields the familiar equation τ = M q¨ + b + g − T λ,

(A.32)

˙ q˙ = 0. q¨ + 

(A.33)

subject to

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:13:44, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.013

References

Appell, P. (1900), Sur une forme générale des équations de la dynamique, Journal für die Reine und Angewandte Mathematik 121, 310–319. Baumgarte, J. (1972), Stabilization of constraints and integrals of motion in dynamical systems, Computer Methods in Applied Mechanics and Engineering 1(1), 1–16. Belytschko, T., Liu, W. K., Moran, B. and Elkhodary, K. (2013), Nonlinear Finite Elements for Continua and Structures, John Wiley. Blajer, W. (1997), A geometric unification of constrained system dynamics, Multibody System Dynamics 1(1), 3–21. Clavel, R. (1991), Conception d’ un robot parallèle rapide à 4 degrés de liberté, École Polytechnique Fédérale de Lausanne (EPFL). de Groot, J. H. and Brand, R. (2001), A three-dimensional regression model of the shoulder rhythm, IEEE Transactions on Biomedical Engineering 16, 735–743. De Sapio, V., Holzbaur, K. and Khatib, O. (2006), The control of kinematically constrained shoulder complexes: Physiological and humanoid examples, in Proceedings of the 2006 IEEE International Conference on Robotics and Automatio, Vol. 1–10, IEEE, pp. 2952–2959. De Sapio, V. and Khatib, O. (2005), Operational space control of multibody systems with explicit holonomic constraints, in Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Vol. 1–4, IEEE, pp. 2961–2967. De Sapio, V., Khatib, O. and Delp, S. (2005), Simulating the task-level control of human motion: A methodology and framework for implementation, The Visual Computer 21(5), 289–302. De Sapio, V., Khatib, O. and Delp, S. (2006), Task-level approaches for the control of constrained multibody systems, Multibody System Dynamics 16(1), 73–102. Delp, S. L., Anderson, F. C., Arnold, A. S., Loan, P., Habib, A., John, C. T., Guendelman, E. and Thelen, D. G. (2007), Opensim: Open-source software to create and analyze dynamic simulations of movement, IEEE Transactions Biomedical Engineering 54(11), 1940–1950. Delp, S. L. and Loan, J. P. (2000), A computational framework for simulating and analyzing human and animal movement, IEEE Computing in Science and Engineering 2(5), 46–55. Delp, S. L., Loan, J. P., Zajac, F. E., Topp, E. L. and Rosen, J. M. (1990), An interactive graphicsbased model of the lower extremity to study orthopaedic surgical procedures, IEEE Transactions on Biomedical Engineering 37, 757–767. Denavit, J. and Hartenberg, R. (1955), A kinematic notation for lower-pair mechanisms based on matrices, Journal of Applied Mechanics 23, 215–221. Dirac, P. A. M. (1958), Generalized hamiltonian dynamics, Proceedings of the Royal Society of London, Series A 246, 326–332. Do Carmo, M. P. (1976), Differential Geometry of Curves and Surfaces, Prentice Hall. Dugas, R. (1988), A History of Mechanics, Dover.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:32:17, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.014

276

References

Flannery, M. R. (2005), The enigma of nonholonomic constraints, American Journal of Physics 73(3), 265–272. Gauss, K. F. (1829), Über ein neues allgemeines grundgesetz der mechanik [On a new fundamental law of mechanics], Journal für die Reine und Angewandte Mathematik 4, 232–235. Gibbs, J. W. (1879), On the fundamental formulae of dynamics, American Journal of Mathematics 2(1), 49–64. Goldstein, H., Poole, C. and Safko, J. (2002), Classical Mechanics, Addison Wesley. Hertz, H. (1894), Die prinzipien der mechanik in neuem zusammenhange dargestellt, Barth. Holzbaur, K. R. S., Murray, W. M. and Delp, S. L. (2005), A model of the upper extremity for simulating musculoskeletal surgery and analyzing neuromuscular control, Annals of Biomedical Engineering 33(6), 829–840. Hughes, T. (2000), The Finite Element Method: Linear Static and Dynamic Finite Element Analysis, Dover. Huston, R. L., Liu, C. Q. and Li, F. (2003), Equivalent control of constrained multibody systems, Multibody System Dynamics 10(3), 313–321. Jourdain, P. E. B. (1909), Note on an analogue of Gauss’ principle of least constraint, Quarterly Journal of Pure and Applied Mathematics 40, 153–157. Jungnickel, U. (1994), Differential-algebraic equations in Riemannian spaces and applications to multibody system dynamics, ZAMM 74(9), 409–415. Kane, T. R. (1961), Dynamics of nonholonomic systems, Journal of Applied Mechanics 28(4), 574–578. Khatib, O. (1987), A unified approach to motion and force control of robot manipulators: The operational space formulation, International Journal of Robotics Research 3(1), 43–53. Khatib, O. (1995), Inertial properties in robotic manipulation: An object level framework, International Journal of Robotics Research 14(1), 19–36. Klopˇcar, N. and Lenarˇciˇc, J. (2001), Biomechanical considerations on the design of a humanoid shoulder girdle, in Proceedings of the 2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Vol. 1, IEEE, pp. 255–259. Kübler, L., Eberhard, P. and Geisler, J. (2003a), Flexible multibody systems with large deformations and nonlinear structural damping using absolute nodal coordinates, Nonlinear Dynamics 34(1–2), 31–52. Kübler, L., Eberhard, P. and Geisler, J. (2003b), Flexible multibody systems with large deformations using absolute nodal coordinates for isoparametric solid brick elements, in Proceedings of the 2003 ASME Design Engineering Technical Conference, ASME, pp. 1–10. Lanczos, C. (1986), The Variational Principles of Mechanics, Dover. Lenarˇciˇc, J., Staniši´c, M. M. and Parenti-Castelli, V. (2000), Kinematic design of a humanoid robotic shoulder complex, in Proceedings of the 2000 IEEE International Conference on Robotics and Automation, Vol. 1, IEEE, pp. 27–32. Lenarˇciˇc, J. and Staniši´c, M. M. (2003), A humanoid shoulder complex and the humeral pointing kinematics, IEEE Transactions on Robotics and Automation 19(3), 499–506. Naudet, J., Lefeber, D., Daerden, F. and Terze, Z. (2003), Forward dynamics of open-loop multibody mechanisms using an efficient recursive algorithm based on canonical momenta, Multibody System Dynamics 10(1), 45–59. Papastavridis, J. G. (2002), Analytical Mechanics: A Comprehensive Treatise on the Dynamics of Constrained Systems for Engineers, Physicists, and Mathematicians, Oxford University Press. Rodriguez, G., Jain, A. and Kreutz-Delgado, K. (1991), A spatial operator algebra for manipulator modeling and control, International Journal of Robotics Research 10(4), 371–381.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:32:17, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.014

References

277

Russakow, J., Khatib, O. and Rock, S. M. (1995), Extended operational space formulation for serial-to-parallel chain (branching) manipulators, in Proceedings of the 1995 International Conference on Robotics and Automation, Vol. 1, IEEE, pp. 1056–1061. Saul, K. R., Hu, X., Goehler, C. M., Vidt, M. E., Daly, M., Velisar, A. and Murray, W. M. (2015), Benchmarking of dynamic simulation predictions in two software platforms using an upper limb musculoskeletal model, Computer Methods in Biomechanics and Biomedical Engineering 18(13), 1445–1458. Schutte, L. M. (1992), Using musculsokeletal models to explore strategies for improving performance in electrical stimulation-induced leg cycle ergometry, PhD thesis, Stanford University. Shabana, A. (1998), Computer implementation of the absolute nodal coordinate formulation for flexible multibody dynamics, Nonlinear Dynamics 16(3), 293–306. Udwadia, F. and Kalaba, R. E. (1992), A new perspective on constrained motion, in Proceedings: Mathematical and Physical Sciences, Vol. 439, Royal Society of London, pp. 407–410. Vujanovic, B. D. and Atanackovic, T. M. (2004), An Introduction to Modern Variational Techniques in Mechanics and Engineering, Springer Science & Business Media. Wehage and Haug, E. J. (1982), Generalized coordinate partitioning for dimension reduction in analysis of constrained dynamic systems, Journal of Mechanical Design 104(1), 247–255. Zajac, F. E. (1989), Critical reviews in biomedical engineering, in J. R. Bourne, ed., Muscle and Tendon: Properties, Models, Scaling, and Application to Biomechanics and Motor Control, CRC Press, pp. 359–411. Zajac, F. E. (1993), Muscle coordination of movement: A perspective, Journal of Biomechanics 26, 109–124.

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:32:17, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.014

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:32:17, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301.014

Index

analytical dynamics software, 263–268 dedicated multibody dynamics software, 267–268 general purpose mathematical software, 263–267 angular acceleration, 60 angular momentum, 86–93 angular velocity, 46, 79 angular velocity addition, 52–54 angular velocity tensor, 46 Appell, Paul, 4 arc length, 23–24, 32–33 axis-angle representation, 38–39 biomechanical systems, 235–262 Bizzarie di Varie Figure, 1 Braccelli, Giovanni Battista, 1 branching chain devices, 5 calculus of rotations, 44–54, 79 angular velocity, 46, 52–54, 79 Euler angles, 47–48 quaternion, 46–47 rotation matrix, 45–46 center of mass, 85 centrifugal and Coriolis vector, 111 configuration space, 9, 70, 76–77 generalized coordinates, 3, 9, 70, 76–77, 105, 114, 142, 143, 145, 146 coning angle, 64, 69 conservation of angular momentum, 84 conservation of translational momentum, 84 conservation principles, 83–100 Newton’s Laws, 83–84 Newton-Euler Principle, 83–99 constrained biomechanical systems, 245–262 muscle-based actuation, 246–253 constrained Hamiltonian systems, 143–145, 150 constrained least action, 140–142 constrained-motion manifold, 116 constrained muscle-based actuation, 246–253 constrained task space dynamics, 227–232, 233–234 direct mapping of constraints, 227–229 task/constraint partitioning, 229–232, 233–234

constraint matrix, 115 constraints, 76–77, 81–82, 101, 113–128, 140, 148–149, 150, 151, 160–178, 185–187, 188, 197–200, 211–213, 224, 226, 227–232, 233–234 holonomic constraints, 113–128, 148–149, 150, 211–213, 227–232, 233–234 nonholonomic constraints, 160–178, 185–187, 197–200, 213 rheonomic constraints, 101, 224, 226 scleronomic constraints, 101, 128, 140, 151, 188 covariant derivative, 26 curvature tensor, 25, 35 curves, 23–24, 32–33 arc length, 23–24, 32–33 extrinsic curvature, 24, 32–33 tangent vector, 23, 32–33 d’Alembert’s Principle, 2, 101–128, 147–149 constraints, 113–128 single particle, 101–102 single rigid body, 102–104 system of particles, 104–106 system of rigid bodies, 106–113 virtual work, 107 d’Alembert, Jean Le Rond, 2 dedicated multibody dynamics software, 267–268 commercial software, 267–268 open source software, 268 deformation gradient, 272 degrees of freedom, 76–77, 81–82 Delta mechanism, 5 Denavit-Hartenberg parameters, 69–70, 79–80 derivatives in rotating reference frames, 59–61 diffeomorphism, 23, 269 differential geometry, 22–28, 32–35, 116, 269 curves, 23–24, 32–33 k-manifolds, 23, 116, 269 surfaces, 24–28, 33–35 double pendulum, 5 dynamically consistent inverse, 216

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:35:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

280

Index

eigenbasis, 18–19 eigenspace, 18 eigenvalue problem, 18–19, 32 elimination of Lagrange multipliers, 145–147 Euler angles, 39–41, 78 Euler’s disk, 7 Euler, Leonhard, 1 Euler-Lagrange equations, 3, 128–140, 149–150 kinetic energy ellipsoid, 131–132 single particle, 130 single rigid body, 130–132 system of particles, 132–134 system of rigid bodies, 134–140, 149–150 extrinsic curvature, 24, 32–33 first-order nonholonomic constraints, 160–178 rolling ball, 168–172 rolling disk, 163–168 two-wheeled assembly, 172–178 first-order variational principles, 151, 187 Jourdain’s Principle, 151–178 Kane’s method, 179–184 virtual power, 151, 152, 154, 156, 157, 158, 159 four-bar linkage, 5, 6, 148, 211–213 Gauss map, 24, 33–35 Gauss’s Principle, 188, 200 constraints, 197–200 single particle, 188–189 single rigid body, 189–191 system of particles, 191–193 system of rigid bodies, 193–197 Gauss’s Principle of Least Constraint, 4 Gauss, Carl Friedrich, 4 Gaussian curvature, 25, 35 general purpose mathematical software, 263–267 packages for multibody dynamics, 263–267 generalized accelerations, 4, 111, 192, 197, 201, 202 generalized constrained equation of motion, 127–128 generalized constraint forces, 115, 116, 121, 145, 201 generalized coordinates, 3, 9, 70, 76–77, 105, 114, 142, 143, 145, 146 generalized forces, 94, 105, 133, 134, 147 generalized momenta, 142 generalized velocities, 4, 114, 142, 143, 144, 155, 159, 179, 182, 192, 207 geodesic curvature, 26–27, 33 geodesics, 26–28, 35, 213 covariant derivative, 26 geodesic curvature, 26–27, 33 Gibbs, Josiah Willard, 4 Gibbs-Appell method, 4, 206–210, 213 system of rigid bodies, 206–210 Gram-Schmidt orthogonalization, 16, 31

gravity vector, 111 gyroscope, 5, 135–140 Hamilton’s canonical equations, 142–143, 150 Hamilton’s Principle, 128–142 constrained least action, 140–142 Euler-Lagrange equations, 128–130 Lagrangian, 128 Hamilton, William Rowan, 3 Hamiltonian, 143 Hamiltonian dynamics, 142–145, 150 constrained dynamics, 143–145, 150 Hamiltonian, 143 unconstrained dynamics, 142–143, 150 Hertz’s Principle of Least Curvature, 4, 202–206 Hertz, Heinrich Rudolf, 4 historical background, 1–4 Hoberman Sphere, 6 holonomic constraints, 76–77, 81–82, 113–128, 148–149, 150, 211–213, 227–232, 233–234 constrained-motion manifold, 116 constraint matrix, 115 four-bar linkage, 148, 211–213 Lagrange multipliers, 116 parallel shoulder mechanism, 253–262 planar parallel mechanism, 122–126, 230–232 slider-crank, 148–149, 150, 233–234 Stewart platform, 118–122 holonomically constrained devices, 5–6 Delta mechanism, 5 four-bar linkage, 5, 6, 148, 211–213 Hoberman Sphere, 6 parallel shoulder mechanism, 253–262 planar parallel mechanism, 122–126, 230–232 slider-crank, 6, 148–149, 150, 233–234 Stewart platform, 118–122 homogeneous transform matrix, 54–55 inertia tensor, 88 inertial properties, 85–93, 99–100 center of mass, 85–93, 99–100 inertia tensor, 85–93, 99–100 inertial reference frame, 83 inner product, 10–12 Jacobian matrix, 70–72, 80–81 relationship with screws, 72 Jourdain’s Principle, 4, 151–178, 185–187 constraints, 160–178 single particle, 151–152 single rigid body, 152–154 system of particles, 154–156 system of rigid bodies, 156–160 virtual power, 151, 152, 154, 156, 157, 158, 159 Jourdain, Philip E. B., 4

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:35:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

Index

281

k-manifolds, 23, 116, 269 constrained-motion manifold, 116 diffeomorphism, 23, 269 Kane’s method, 4, 179–184, 185 Kane, Thomas, 4 kinematic chains, 5, 69–76, 79–81, 94–99, 100, 111–113, 135–140, 147–148, 149–150, 217–219, 220–222, 233 Denavit-Hartenberg parameters, 69–70, 79–80 double pendulum, 5 gyroscope, 5, 135–140 Jacobian matrix, 70–72, 80–81 rate vector propagation, 72–76, 81 serial chain robot, 81, 94–99, 100, 111–113, 147–148, 149–150, 217–219, 220–222, 233 tree-structured mechanism, 5 kinematics, 36–82 degrees of freedom, 76–77, 81–82 kinematic chains, 69–76, 79–81 spatial kinematics, 54–69, 79 spherical kinematics, 36–54, 77–79 kinetic energy, 130, 131, 133, 134, 204 kinetic energy ellipsoid, 131–132

Newton’s Laws, 83–84 Newton, Isaac, 1 Newton-Euler Principle, 83–99, 100 conservation of angular momentum, 84 conservation of translational momentum, 84 Newton’s Laws, 83–84 single particle, 84–85 single rigid body, 85–93 system of particles, 93 system of rigid bodies, 93–99 nonholonomic constraints, 160–178, 185–187, 197–200, 213 first-order nonholonomic constraints, 160–178 rolling disk, 185, 213 second-order nonholonomic constraints, 197–200 two-wheeled assembly, 185–187 nonholonomically constrained devices, 6–7 Euler’s disk, 7 rolling ball, 7, 168–172 rolling disk, 163–168, 185, 213 two-wheeled assembly, 7, 172–178, 185–187 normal curvature, 27, 33 null space projection matrix, 216

Lagrange multipliers, 116, 145 Lagrange, Joseph-Louis, 3 Lagrangian, 128, 142, 204 least norm, 17–18, 31–32 least squares, 16–17, 31 Leibniz, Gottfried, 2 linear systems, 10–22, 31–32 tensors in R3 , 21 vector and tensor operations, 21–22 vector spaces in Rn , 10–19, 31–32 vectors in R3 , 19–21

orthogonal complement, 15–16 orthogonal projection, 16, 31

mass matrix, 111 mean curvature, 25, 35 motors, 57–58 spatial velocity, 57, 72, 79 muscle activation dynamics, 236 muscle contraction dynamics, 236–238 pennation angle, 236 muscle Jacobian, 236 muscle moment arm matrix, 236 musculoskeletal dynamics, 235–236 muscle Jacobian, 236 muscle moment arm matrix, 236 neuro-musculoskeletal dynamics, 235–243 musculoskeletal dynamics, 235–236 neuromuscular dynamics, 236–238 stiff tendon model, 238–243 neuromuscular dynamics, 236–238 activation dynamics, 236 contraction dynamics, 236–238

parallel shoulder mechanism, 253–262 pennation angle, 236 Piola-Kirchoff stress tensor, 272 planar parallel mechanism, 122–126, 230–232 positive definite matrix, 19, 88, 111, 131, 142, 216 potential energy, 130, 131, 133, 134 principal curvatures, 25, 35 Principle of Least Constraint, 200–206 Hertz’s Principle of Least Curvature, 202–206 system of rigid bodies, 201–202 propagation of kinematic rate vectors, 72–76 quasi-accelerations, 115, 207, 208, 211 quasi-forces, 180, 208 quasi-Jacobians, 179, 207 quasi-velocities, 115, 179, 180, 182, 207, 210 quaternion derivative, 46–47 quaternions, 41–44 rheonomic constraints, 101 rolling ball, 7, 168–172 rolling disk, 163–168, 185, 213 rotation matrices, 36–38 rotation matrix derivative, 45–46 scleronomic constraints, 101, 128, 140, 151, 188 screws, 55–57, 79 second-order nonholonomic constraints, 197–200

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:35:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301

282

Index

second-order variational principles, 188, 213 Gauss’s Principle, 188, 200 Gibbs-Appell method, 206–211 Principle of Least Constraint, 200–206 serial chain robot, 81, 94–99, 100, 111–113, 147–148, 149–150, 217–219, 220–222, 233 slider-crank, 6, 148–149, 150, 233–234 spatial kinematics, 54–69, 79 derivatives in rotating reference frames, 59–61 homogeneous transform, 54–55 motors, 57–58, 79 screws, 55–57, 79 wrenches, 58–59 spatial velocity, 57, 72, 79 spherical kinematics, 36–54, 77–79 axis-angle, 38–39 calculus of rotations, 44–54, 79 Euler angles, 39–41, 78 quaternions, 41–44 rotation matrices, 36–38 static optimization, 28–30 Stewart platform, 118–122 stiff tendon model, 238–243 surface traction, 272 surfaces, 24–28, 33–35, 213 curvature tensor, 25, 35 Gauss map, 24, 33–35 Gaussian curvature, 25, 35 geodesic curvature, 26–27, 33 geodesics, 26–28, 35, 213 mean curvature, 25, 35 normal curvature, 27, 33 principal curvatures, 25, 35 tangent vector, 23, 32–33 task space belted ellipsoid, 220 task space centrifugal and Coriolis vector, 216 task space dynamics, 214–234 belted ellipsoid, 220 centrifugal and Coriolis vector, 216 constrained task space dynamics, 227–232 dynamically consistent inverse, 216 energy minimization, 223–224 gravity vector, 216 inertial properties, 219–222 kinetic energy ellipsoid, 220

kinetic energy minimization, 223–224 least action, 226–227 mass matrix, 216 null space projection matrix, 216 potential energy minimization, 223 serial chain robot, 217–219 task/constraint symmetry, 224–227 task space gravity vector, 216 task space kinetic energy, 219 task space kinetic energy ellipsoid, 220 task space mapping of constraints, 227–229 task space mass matrix, 216 task/constraint partitioning, 229–232, 233–234 task/constraint symmetry, 224–227 tensors in R3 , 21 tree-structured mechanism, 5 two-wheeled assembly, 7, 172–178, 185–187 variational principles, 101–213 first-order variational principles, 151, 187 second-order variational principles, 188–213 zeroth-order variational principles, 101–150 vector and tensor operations, 21–22 vector basis, 12–15 vector spaces in Rn , 10–19, 31–32 eigenbasis, 18–19 eigenspace, 18 eigenvalue problem, 18–19, 32 Gram-Schmidt, 16, 31 inner product, 10–12 least norm, 17–18, 31–32 least squares, 16–17, 31 orthogonal complement, 15–16 orthogonal projection, 16, 31 vector basis, 12–15 vectors in R3 , 19–21 virtual power, 151, 152, 154, 156, 157, 158, 159 virtual work, 107 vis viva, 2 wrenches, 58–59 zeroth-order variational principles, 101–150 d’Alembert’s Principle, 101–128 Hamilton’s Principle, 128–142 virtual work, 107

Downloaded from https:/www.cambridge.org/core. University College London, on 21 Mar 2017 at 17:35:49, subject to the Cambridge Core terms of use, available at https:/www.cambridge.org/core/terms. https://doi.org/10.1017/9781316832301